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ABSTRACT 


The  main  problem  addressed  by  this  research  is  the  lack  of  a  small,  low-cost  integrated  naviga¬ 
tion  system  to  accurately  determine  the  position  of  an  Autonomous  Underwater  Vehicle  (AUV) 
during  all  phases  of  an  underwater  search  or  mapping  mission.  The  approach  taken  utilized  an 
evolving  prototype,  called  the  Shallow- Water  AUV  Navigation  System  (SANS),  combining  Glo¬ 
bal  Positioning  System  (GPS),  Inertial  Measurement  Unit  (IMU),  water  speed,  and  magnetic 
heading  information  using  Kalman,  low-pass,  and  complimentary  filtering  techniques.  In  previ¬ 
ous  work,  addition  of  a  math  coprocessor  improved  system  update  rate  from  7  to  18  Hz,  but 
revealed  inpul/output  coordination  weaknesses  in  the  software.  The  central  focus  of  this  thesis  is 
on  testing  and  programming  improvements  which  resulted  in  reliable  integrated  operations  and  an 
increased  processing  speed  of  40  Hz.  This  now  allows  the  filter  to  perform  in  real  time.  A  stan¬ 
dardized  tilt  table  evaluation  and  calibration  procedure  for  the  navigation  filter  also  was  devel¬ 
oped. 

The  system  was  evaluated  in  dynamic  tilt  table  experiments.  Test  results  and  qualitative  error 
estimates  using  differential  GPS  suggest  that  submerged  navigation  with  SANS  for  a  period  of 
several  minutes  will  result  in  position  estimation  errors  typically  on  the  order  of  10  meters  rms, 
even  in  the  presence  of  substantial  ocean  currents. 
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1.  INTRODUCTION 


A.  BACKGROUND 

Autonomous  Underwater  Vehicles  (AUVs)  are  capable  of  a  variety  of  overt  and 
clandestine  missions.  Such  vehicles  have  been  proposed  for  inspection,  mine 
countermeasures,  survey,  and  observation.  Recent  research  trends  in  underwater  robotics 
have  emphasized  minimizing  the  need  for  human  interaction  by  increasing  AUV 
autonomy.  (Yuh  95) 

The  NFS  Phoenix  AUV  is  an  experimental  vehicle  designed  primarily  for  research  in 
support  of  shallow-water  mine  countermeasures  and  coastal  environmental  monitoring 
(Healey  93,  95,  Brutzman  96).  The  clandestine  nature  of  the  missions  for  which  Phoenix 
was  designed  necessitates  minimum  surfaced  exposure  time  while  in  the  operating  area,  the 
ability  to  submerge  in  order  to  investigate  targets,  and  a  navigation  system  that  is  accurate 
enough  to  allow  target  revisit  if  desired. 

Many  missions  of  the  Phoenix  class  of  vehicles  can  be  separated  into  two  distinct 
phases:  transit  and  search.  After  being  launched  from  an  aircraft,  submarine,  or  surface 
vessel,  the  AUV  would  execute  a  transit  phase  in  order  to  arrive  at  the  search  area.  Once 
established  in  the  mission  area,  it  would  enter  a  search  phase,  which  might  include  missions 
such  as  mine  hunting,  mapping,  or  environmental  data  collection.  Navigation  is  one  of  the 
most  important  and  difficult  aspects  of  an  AUV  mission.  Therefore,  a  robust,  real-time 
navigation  system  is  critical  for  a  multi-mission  capable  AUV.  Typically,  a  search  phase 
would  require  more  precise  navigation  than  a  transit  phase.  This  could  be  accomplished  by 
obtaining  more  frequent  Global  Positioning  System  (GPS)  fixes,  or  by  using  Differential 
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GPS  (DGPS)  either  in  real-time  if  available,  or  after  mission  completion  using  post¬ 
processing  (Walker  96).  After  the  search  is  completed,  the  AUV  would  commence  a 
second  transit  phase  and  return  to  a  recovery  position.  Both  kinds  of  mission  phases  would 
typically  involve  waypoint  steering,  and  possibly  obstacle  avoidance. 

An  approach  is  described  in  Kwak  (93)  for  determining  the  position  of  submerged 
detected  objects  by  executing  a  “pop-up”  maneuver  to  obtain  a  GPS  fix.  This  fix  is  then 
extrapolated  backwards  to  the  submerged  object  location  using  recorded  inertial  data. 
Navigation  accuracy  during  such  a  surfacing  maneuver  is  strongly  enhanced  by  the  use  of 
accurate  depth  information  available  from  low-cost  pressure  cells  (Kwak  93).  However, 
this  form  of  “aided”  inertial  navigation  is  not  applicable  to  a  surfaced  or  near  surface  AUV 
(Brown  92). 

Continuously  reliable  GPS  reception  would  not  be  possible  unless  the  AUV  were  to 
be  fitted  with  an  extensible  mast  mounted  antenna.  Extending  an  antenna  above  the  effects 
of  wave  action  is  not  desirable  for  a  military  application  and,  at  any  rate,  would  probably 
be  mechanically  impractical  for  a  small  AUV.  As  a  result,  any  system  relying  solely  upon 
GPS  would  not  be  sufficiently  robust  to  provide  accurate  navigation  information  during 
surfaced  or  near  surface  operations  due  to  intermittent  reception.  Therefore,  inertial 
navigation  is  needed  between  periods  where  continuous  reliable  reception  of  GPS  satellite 
signals  is  not  possible.  (Bachmann  95) 

Inertial  navigation  hardware  is  sometimes  based  on  rotating  gyros,  which  provide 
attitude  information  needed  to  stabilize  a  platform  that  holds  acceleration  sensors.  The 
limiting  factors  to  this  approach  include:  high  expense  due  to  required  precision,  inordinate 


power  consumption,  high  failure  rates,  and  acoustic  and  structure-borne  noise  (Cox  94). 
These  factors  counter  the  Phoenix  AUV  philosophy  of  providing  a  low  cost,  general 
purpose  platform  capable  of  long-term  independent  operation,  despite  relatively  small 
vehicle  size  (McGhee  95).  Additionally,  the  rotating  gyros  now  installed  in  Phoenix  are 
aging  and  mechanically  unreliable.  It  is  therefore  desirable  to  find  a  solution  to  the  AUV 
navigation  and  control  problem  not  requiring  such  components. 

In  order  to  achieve  robust  navigation,  the  AUV  should  be  capable  of  navigating  with 
GPS  and/or  an  Inertial  Navigation  System  (INS).  GPS  is  capable  of  highly  accurate 
positioning  when  the  AUV  is  surfaced,  while  an  INS  can  be  used  for  submerged  navigation 
and  periods  between  GPS  satellite  reception.  In  order  to  ensure  accurate  navigation  for  a 
wide  variety  of  missions,  GPS  and  ENS  components  can  be  combined.  A  favorable  analysis 
of  this  type  of  navigation  system  was  conducted  in  McKeon  (92).  The  hardware  and 
software  architecture  required  for  a  typical  mapping  scenario  was  evaluated  in  Norton  (94). 

Bachmann  (95)  made  the  architecture  evaluated  in  Norton  (94)  a  reality,  and 
subsequently  developed  the  first  working  protot5q)e  of  the  proposed  Shallow-Water  AUV 
Navigation  System  (SANS).  The  SANS  was  designed  to  overcome  the  problem  of 
intermittent  GPS  satellite  tracking.  It  is  an  experimental  system  that  uses  a  low-cost, 
strapped-down  inertial  measurement  unit  (IMU),  complemented  with  magnetic  heading 
and  water  speed  sensors,  to  enable  inertial  navigation  between  GPS  fixes.  This  system  is 
well  suited  for  pop-up  navigation.  Finding  this  means  of  navigating  near  the  sea  surface 
provides  a  complete  solution  to  the  overall  navigation  problem  associated  with  tiansiting 
an  AUV  to  a  shallow  water  work  site,  recording  the  position  of  detected  submerged  objects. 
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and  then  returning  to  a  recovery  site  where  stored  mission  data  can  be  uploaded  (McGhee 
95). 

Additionally,  the  navigation  filter  developed  by  McGhee  (95)  solves  the  problems  of 
cost  and  power  consumption  by  eliminating  rotating  gjros  and  replacing  them  with 
acceleration  and  angular  rate  sensors.  This  filter  is  implemented  in  SANS  by  Bachmann 
(95).  One  application  of  SANS  is  to  upgrade  the  Phoenix  navigation  system.  Others, 
particularly  as  component  miniaturization  continues,  include  marine  mammal  and  diver 
navigation. 

With  the  prototype  SANS  having  achieved  favorable  results  in  open-water,  at-sea  test 
trials,  Walker  (96)  advanced  the  SANS  to  another  level  of  maturity,  making  it  a  truly 
integrated  system  ready  for  direct  application  to  a  real-world  AUV.  The  physically 
redesigned  system  includes  an  on-board  processor  and  consolidated  the  diverse 
components  into  a  compact  unit,  while  improving  individual  component  reliability  and 
performance.  The  research  reported  in  this  thesis  continues  the  evolution  of  the  SANS  by 
incorporating  software  improvements  to  accommodate  the  dramatically  improved 
processing  speed,  implementing  a  networking  capability  to  monitor  at-sea  tests  and  prepare 
for  installation  into  the  AUV,  and  developing  a  standardized  calibration  procedure  for  the 
navigation  filter. 

B.  RESEARCH  QUESTIONS 

This  thesis  will  examine  the  following  research  topics: 

-  Evaluate  the  hardware  and  software  architecture  of  the  SANS. 

-  Develop  a  calibration  procedure  for  the  SANS  navigation  filter. 
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-  Evaluate  the  performance  of  the  SANS  navigation  filter  in  a  laboratory  environment. 

-  Evaluate  the  SANS  hardware  and  software  architecture  for  installation  in  Phoenix. 

C.  SCOPE,  LIMITATIONS  AND  ASSUMPTIONS 

This  thesis  reports  part  of  the  findings  of  the  fifth  year  of  research  in  an  ongoing 

research  project.  The  scope  of  this  thesis  is  to  evaluate  SANS  attitude  estimation 
capabilities  for  eventual  installation  as  a  replacement  for  the  older  technology  gyros  now 
used  on  board  the  Phoenix  AUV.  The  requirements  for  an  ideal  SANS  described  by  Kwak 
(93)  which  impact  this  project  are; 

-  Low  power  consumption.  Operation  from  a  small  external  battery  pack  for  12  hours 
is  desirable. 

-  Limited  exposure  time.  The  amount  of  time  that  the  GPS  antenna  is  exposed  in  the 
search  phase  should  be  as  short  as  possible.  Up  to  30  seconds  of  exposure  is  allowed, 
but  less  is  better,  and  time  between  exposures  should  be  maximized. 

-  Maintain  clandestine  operation.  The  GPS  antenna  should  present  a  very  small  cross 
section  when  exposed  and  should  not  extend  more  than  a  few  inches  above  the 
surface  of  the  water. 

-  Maximize  accuracy.  During  the  search  phase  of  the  mission,  system  accuracy  of  10 
meters  or  better  is  required  following  postprocessing,  both  while  submerged  and 
surfaced. 

-  Total  volume  not  to  exceed  120  cubic  inches.  Elongated,  streamlined  packaging  is 
preferred. 
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D.  ORGANIZATION  OF  THESIS 

The  purpose  of  this  thesis  is  to  present  the  development  of  a  prototype  system  intended 
to  meet  the  mission  requirements  of  the  SANS.  The  term  AUV  is  understood  to  include 
any  small  underwater  vehicle  (including  human  divers)  which  can  easily  carry  such  a 
compact  device.  The  term  “towfish”  refers  to  the  test  vehicle  used  to  evaluate  the  SANS 
during  at-sea  testing. 

This  thesis  provides  an  evaluation  of  the  hardware  and  software  used  to  provide 
accurate  navigation  for  the  NPS  AUV.  The  major  thrust  of  the  thesis  is  to  evaluate  the 
attitude  estimation  capabilities  of  the  SANS  both  statically  and  dynamically  in  a  laboratory 
environment. 

Chapter  II  reviews  previous  work  on  this  project  as  well  as  on  GPS  and  INS 
navigation,  AUV  submerged  navigation,  and  navigation  filtering  theory.  Chapter  IE 
provides  a  summary  description  of  both  the  original  and  current  SANS  prototype  hardware. 
Chapter  IV  provides  a  detailed  description  of  the  software  architecture,  including  the 
navigation  filter.  Particular  emphasis  is  placed  on  changes,  additions,  and  updates  made  to 
the  C-4H-  code  in  support  of  this  portion  of  the  project.  Chapter  V  is  a  description  of  the 
experiment  design  and  an  analysis  of  the  experimental  results.  Finally,  Chapter  VI  presents 
the  thesis  conclusions  and  provides  recommendations  for  future  research. 
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11.  SURVEY  OF  RELATED  WORK 


A.  INTRODUCTION 

Autonomous  Underwater  Vehicles  (AUVs)  have  the  potential  to  be  used  in  an  efficient 
and  cost  effective  manner  in  a  variety  of  missions  involving  military  and  non-military 
applications.  Accurate  navigation  is  one  of  the  most  important  capabilities  supporting 
AUV  mission  effectiveness.  Many  possible  AUV  missions,  such  as  mine  hunting,  require 
a  high  degree  of  navigation  accuracy.  This  chapter  will  discuss  some  of  the  possible  AUV 
navigation  solutions. 

Navigation  systems  are  generally  categorized  by  whether  they  are  based  on  external 
signal  reception  or  internal  sensors.  Extemal-signal-based  navigation  systems,  such  as 
Loran,  Omega,  and  Global  Positioning  System  (GPS),  are  limited  to  determining  position 
only  while  the  receiver  is  exposed  to  the  signal.  Loran  and  Omega  are  relatively  inaccurate 
compared  to  GPS.  While  Loran  covers  most  of  the  northern  hemisphere,  it  has  almost  no 
coverage  in  the  southern  hemisphere  (Bowditch  84).  GPS  provides  an  attractive, 
affordable  system  for  the  surfaced  portion  of  an  AUV  mission  because  it  is  capable  of 
world-wide  coverage  with  a  high  degree  of  navigational  accuracy. 

Intemal-sensor-based  navigation  can  be  implemented  as  a  self-contained  unit  which 
can  be  composed  of  various  types  of  equipment  such  as  inertial  measuring  units  (IMUs), 
acoustic  transponders,  or  geophysical  map  comparison.  AU  sensors  are  subject  to  some 
amount  of  error,  which  may  compound  to  unacceptable  levels  for  some  AUV  missions  if 
not  accounted  for.  Each  of  these  components  also  has  unique  disadvantages.  Acoustic 
transponders  must  be  pre-deployed  at  precisely  known  locations  and  may  require  costly 
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maintenance.  Geophysical  map  interrogation  requires  a  precise  bottom  contour  map  be 
previously  stored  in  the  AUV’s  computer.  IMU-based  navigation  is  prone  to  sensor  drift, 
which  if  left  uncorrected,  can  become  very  large.  However,  it  has  advantages  relative  to 
the  other- navigation  options  due  to  a  lack  of  dependence  on  external  signals  and  no 
requirement  to  transmit  any  signals  which  might  reveal  its  presence. 

B.  GPS  NAVIGATION 

The  Navigation  Satellite  Timing  and  Ranging  (NAVSTAR)  Global  Positioning  System 
(GPS)  is  a  space-based  radio  positioning,  navigation  and  time-transfer  system  sponsored 
by  the  U.S.  Department  of  Defense  (DoD).  It  was  originally  intended  to  provide  the 
military  with  precise  navigation  and  timing  capabilities  (Parkinson  80).  The  system  is 
designed  to  provide  24-hour,  all-weather  navigation  anywhere  on  earth.  It  is  comprised  of 
24  satellites  in  22,200  km  orbits  that  are  inclined  at  55°  to  the  earth’s  spin  axis,  with  12 
hour  periods.  The  satellites  broadcast  two  L-band  frequencies:  LI  (1575.4  MHz)  and  L2 
(1227.6  MHz).  Navigation  and  system  data,  predicted  satellite  position  (ephemeris) 
information,  atmospheric  propagation  correction  data,  satellite  clock  error  information,  and 
satellite  health  data  are  all  superimposed  on  these  two  carrier  frequencies.  (Logsdon  92, 
Wooden  85) 

There  are  two  different  navigation  services  available  from  the  GPS  satellites  depending 
on  the  type  of  receiver  being  used:  the  Standard  Positioning  Service  (SPS),  and  the  Precise 
Positioning  Service  (PPS).  The  SPS  is  based  on  receiving  the  LI  carrier  signal,  which  is 
broadcast  with  an  intentional  inaccuracy  called  Selective  Availability  (SA).  SA  limits 
world-wide  navigation  to  100  m  horizontal  accuracy  with  a  95%  confidence  level  (Logsdon 
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92).  PPS  is  based  on  the  L2  signal.  It  is  limited  to  U.S.  and  allied  military,  and  specific 
non-military  uses  that  are  in  the  national  interest.  Access  to  PPS  is  restricted  by  use  of 
special  cryptographic  equipment.  PPS  provides  the  highest  stand  alone  accuracy:  16  m 
Spherical- Error  Probable  (SEP),  a  velocity  accuracy  of  0.1  m/sec,  and  a  timing  accuracy  of 
better  than  100  nanoseconds.  (Logsdon  92,  Wooden  85) 

Civilian  customers  have  determined  a  way  to  improve  the  accuracy  of  the  SPS  in  order 
to  take  full  advantage  of  GPS  precision  without  having  access  to  cryptographic  equipment. 
This  method,  called  Differential  GPS  (DGPS),  provides  a  way  of  working  around  the 
inaccuracies  of  the  SPS.  It  may  be  used  in  real-time  or  during  post-processing.  The  general 
idea  is  to  place  a  receiver  at  a  surveyed  stationary  site.  The  receiver  is  then  able  to 
determine  the  difference  between  its  actual  position  and  its  computed  GPS  position,  and 
broadcast  the  resulting  pseudorange  (distance  to  satellite)  corrections  to  any  DGPS  capable 
receivers.  Real-time  differential  processing  can  reduce  the  typical  100  m  accuracy  of  the 
SPS  to  2-4  m  regardless  of  the  status  of  SA  (Logsdon  92).  It  is  also  possible  to  record  the 
raw  PPS  or  SPS  GPS  information  for  later  comparison  to  a  known  geographical  site  using 
post-processing.  Precise  procedures  can  be  used  to  reconstruct  extremely  accurate 
positioning  information,  t5q)ically  in  the  submeter  range.  Table  1  shows  a  comparison  of 
expected  GPS  accuracies. 

The  size  and  cost  of  GPS  receivers  have  decreased  drastically  as  GPS  technology  has 
matured.  Miniaturization  is  continuously  progressing  while  maintaining  or  increasing  GPS 
receiver  performance  capability.  Since  as  early  as  1992,  the  GPS  industry  has  been  able  to 
produce  receivers  that  are  essentially  a  single  printed  circuit  board.  Souen  (92)  reports 
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POSITIONING  SERVICE 

PPS  (m) 

SPS  (m) 

Non-Differential 

16 

100 

Differential 

2-4 

2-4 

TABLE  1:  Expected  RMS  GPS  Accuracy  Levels  (Logsdon  92) 
that  the  Furuno  GPS  receiver  module  LGN-72  is  an  eight-channel  receiver  implemented  on 
a  single  printed  circuit  board  measuring  100  mm  x  70  mm  x  20  mm  and  requiring  only  2 
W  of  power. 

There  is  currently  a  performance  trade-off  associated  with  the  miniaturization  of  GPS 
receivers.  For  instance,  Trimble  offers  the  PC  Card  1 10  GPS  miniature  receiver  in  the  form 
of  a  Personal  Computer  Memory  Card  International  (PCMCIA)  interface.  This  credit  card¬ 
sized  device  simply  slides  into  any  laptop,  most  palmtops,  or  pen-based  computers 
compliant  with  PCMCIA  (release  2.0).  It  is  capable  of  tracking  eight  satellites  using  three 
channels.  However,  because  it  does  not  have  an  allocated  channel  for  each  of  the  satellites, 
it  does  not  use  a  continuous  tracking  scheme.  This  degrades  its  acquisition  time 
performance.  In  order  to  reduce  receiver  size,  manufacturers  often  reduce  the  number  of 
channels  on  the  receiver.  GPS  receivers  in  this  configuration  are  called  “sequencing” 
receivers  (Logsdon  92).  Sequencing  receivers  utilize  a  time-sharing  technique  to  “dwell” 
on  each  satellite  for  a  brief  interval  before  switching  to  the  next  satellite  in  the  sequence. 
They  have  a  t5^ical  acquisition  time  of  about  two  minutes.  Continuous  tracking  GPS 
receivers  have  typical  acquisition  times  of  about  30  seconds  or  less.  However,  their  larger 
number  of  receiver  channels  results  in  a  less  compact  size.  Given  this  trade-off  between 
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size  and  performance,  the  choice  of  GPS  receiver  must  be  made  with  the  particular 
application  in  mind.  A  sequencing  receiver  offers  an  adequate  compromise  for  applications 
such  as  mobile  navigation  that  are  not  so  dynamic.  However,  if  the  application  requires  a 
short  time  to  initial  acquisition,  the  most  viable  option  is  the  continuous  tracking  receiver. 
GPS  is  an  obvious  choice  for  AUV  navigation  given  the  level  of  miniaturization  and  its 
excellent  accuracy  performance. 

One  manner  of  using  GPS  to  locate  an  AUV  is  to  place  buoys  with  GPS  receivers  at 
appropriate  locations.  These  buoys  would  translate  the  GPS  signal  and  retransmit  an 
underwater  acoustic  signal.  The  AUV  would  then  determine  its  position  via  ranging  and 
position  fixes  to  the  buoys.  Youngberg  (91)  suggests  that  the  GPS  antenna,  receiver, 
processing  and  control  subsystem,  acoustic  transmitter,  battery  power,  and  homing  beacon 
could  all  be  contained  in  a  buoy  measuring  123  mm  diameter  x  910  mm  long  and  weighing 
5  - 15  kg.  A  simulation  which  showed  the  feasibility  of  this  approach  is  presented  in  Leu 
(93).  The  simulation  consisted  of  several  sonobuoys  spaced  one  kilometer  apart.  Due  to 
uncertainties  in  buoy  position  caused  by  wave  action  and  variations  in  altitude,  the  study 
proposed  the  use  of  Kalman  filtering  techniques  to  combine  the  outputs  of  an  accelerometer 
and  DGPS  to  enhance  accuracy.  Each  GPS  buoy  would  essentially  act  as  a  GPS  satellite 
and  broadcast  its  position  via  spread  spectrum  acoustic  signals  used  by  the  AUV  for 
ranging.  This  technique  would  eliminate  the  requirement  to  predeploy  a  surveyed 
transponder  field. 

Another  possible  method  for  using  GPS  to  determine  the  AUV’s  position  is  to 
physically  mount  the  GPS  antenna  and  receiver  on-board  the  AUV.  For  areas  covered  by 
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DGPS  service,  this  has  the  advantage  of  making  the  system  self-contained.  One  major 
concern  would  be  that  the  GPS  receiver  would  be  unable  to  acquire  satellites  in  a  timely 
manner  due  to  splash  effects  on  the  antenna.  However,  Norton  (94)  describes  both  static 
and  dynamic  test  results  which  show  that  a  submersible  system  is  able  to  meet  the  accuracy 
and  time  requirements  of  the  mission,  even  while  being  splashed  by  wave  wash.  Therefore, 
this  method  was  adopted  in  the  SANS  configuration. 

C.  INS  NAVIGATION 

Inertial  navigation  is  essentially  a  complex  method  of  dead  reckoning.  Its  purest  form 
involves  no  outside  references  to  fix  position.  All  position  data  is  calculated  relative  to  a 
known  starting  point.  An  inertial  navigation  system  (INS)  continuously  measures  three 
mutually  orthogonal  acceleration  components  using  accelerometers.  These  measurements 
are  taken  in  short  time  increments  and  multiplied  by  elapsed  time  in  order  to  determine  an 
estimate  of  instantaneous  velocity.  The  three-dimensional  change  in  position  can  then  be 
determined  by  integrating  respective  velocities  over  time.  (Bachmann  95) 

The  primary  drawback  of  any  INS  is  the  tendency  for  small  sensor  drift  rates  to 
accumulate  as  errors  over  time.  Without  outside  references  for  correction,  these  errors 
grow  relentlessly  and  eventually  lead  to  large  errors  in  the  estimated  position.  Highly 
accurate  inertial  navigation  systems  can  be  constructed,  but  they  are  large,  costly,  and 
complex  (Touhy  93).  Size  alone  makes  them  unacceptable  for  the  SANS.  A  compromise 
solution  to  meet  SANS  requirements  is  to  integrate  a  low-cost,  miniature  INS  with  GPS. 
In  such  a  system,  GPS  will  provide  the  INS  with  the  periodic  position  fixes  necessary  to 
correct  slowly  building  INS  errors. 
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The  acceleration  measurements  required  by  an  INS  can  be  made  by  several  types  of 
IMUs.  There  are  two  fundamental  categories:  gimbaled  and  strapdown.  Due  to  their  large 
size  and  power  requirements,  gimbaled  systems  are  not  suitable  for  the  SANS.  In  a 
strapdown  unit,  three  mutually  orthogonal  accelerometers  and  three  angular  rate  sensors 
are  mounted  parallel  to  the  three  body  axes  of  the  vehicle.  Linear  accelerations  and 
rotational  velocities  are  continuously  measured.  Strapdown  systems  are  smaller  and 
simpler  than  gimbaled  systems,  but  necessitate  much  larger  computational  capabilities. 
(Logsdon  92) 

D.  INTEGRATED  GPS/INS  NAVIGATION 

SPS  mode  GPS  navigation  could  be  used  to  adequately  perform  both  the  transit  and 

search  phases  of  an  AUV  mission.  During  surfaced  transit  phases,  non-differential  SPS,  a 
water  speed  sensor,  and  a  magnetic  compass  would  provide  the  primary  source  of 
navigation  data.  In  order  to  utilize  GPS  as  a  meaningful  correction  to  a  low-cost  INS 
system,  periods  between  GPS  fixes  during  the  transit  phase  must  not  exceed  the  time  in 
which  the  INS  error  has  accumulated  to  an  amount  comparable  to  the  horizontal  accuracy 
of  SPS  (100m)  (Bachmann  95).  The  search  or  mapping  phases  of  an  AUV  mission  would 
require  the  vehicle  to  maintain  a  more  accurate  navigational  picture,  both  submerged  and 
on  the  surface.  This  would  necessitate  the  use  of  periodic  differentially  corrected  GPS 
information  in  order  to  keep  the  INS  system  accurate  while  submerged.  This  differential 
correction  could  be  provided  in  real-time  during  overt  missions  along  friendly  shores  where 
a  DGPS  reference  signal  is  available,  or  during  mission  post-processing  following  a 
clandestine  mission. 
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Integration  of  GPS  and  INS  into  a  single  system  can  produce  continuously  accurate 
navigational  information  even  when  using  relatively  low-cost  components.  This 
integration  not  only  allows  periodic  reinitialization  of  the  INS  to  correct  accumulated 
errors,  but  can  also  (with  the  aid  of  Kalman  filtering  techniques)  improve  the  performance 
of  the  INS  between  fixes.  Complementary  filtering  of  acceleration  data  with  additional 
sensor  information  such  as  water  speed  and  heading  wiU  further  improve  system  accuracy. 
Overall,  an  integrated  system  will  provide  improved  reliability,  smaller  navigation  errors, 
and  superior  survivability.  (Logsdon  92) 

Kalman  filtering  is  a  method  of  combining  all  available  sensor  data,  regardless  of  their 
precision,  to  estimate  the  current  posture  of  a  vehicle  (Cox  90).  The  filter  is  actually  a  data- 
processing  algorithm  which  minimizes  the  error  of  this  estimate  statistically  using  currently 
available  sensor  data  and  prior  knowledge  of  system  characteristics.  Each  piece  of  data  is 
weighted  relative  to  data  from  other  system  components  based  upon  the  expected  accuracy 
of  the  measurement  it  represents.  In  a  complementary  filter,  low-frequency  data,  which  is 
trusted  over  the  long  term,  and  high-frequency  data,  which  is  trusted  only  in  the  short  term, 
are  used  to  “complement”  each  other  providing  a  much  better  estimate  than  either  can 
alone.  (Brown  92) 

Bachmann  (95)  demonstrated  the  use  of  the  complementary  filter  technique  by 
combining  low-frequency  orientation  data  from  accelerometers  and  a  magnetic  compass 
with  high-frequency  angular  rate  information  to  estimate  heading  and  attitude. 
Intermediate  position  results  were  obtained  by  integrating  high-frequency  water-speed 
data.  GPS  data  was  used  to  reinitialize  the  system  each  time  a  fix  was  obtained  and  to 


14 


develop  an  error  bias,  expressed  as  an  apparent  ocean  current.  The  current  was  utilized  to 
correct  the  system  between  GPS  fixes.  The  concept  of  using  a  relatively  inexpensive  IMU 
with  limited  accuracy,  coupled  with  differentially-corrected  GPS,  has  proven  to  be  a  viable 
solution  to  the  challenge  of  shaUow-water  AUV  navigation.  (Bachmann  95) 

The  above  conclusion  has  been  independently  duplicated  in  Wolf  (96).  Utilizing  an 
integrated  GPS/INS  system  using  the  same  Systron-Donner  MU  used  in  SANS,  but 
without  incorporating  DGPS,  accuracies  in  attitude  of  better  than  0.2*  in  roll  and  pitch  and 
0.3°  in  azimuth  were  achieved.  Specific  results  from  those  tests,  along  with  static  tests 
indicating  the  SANS  software  filter  (described  in  Chapter  IV)  response  to  MU  inputs  are 
discussed  further  in  Chapter  VI,  System  Testing.  (Wolf  96) 

E.  AUV  SUBMERGED  NAVIGATION 

There  are  many  techniques  available  for  submerged  navigation,  including  dead 

reckoning,  inertial,  electromagnetic,  and  acoustic  navigation.  With  acoustic  navigation, 
time  of  arrival  and  direction  of  propagation  of  acoustic  waves  are  the  two  principal 
measurements  made.  A  wide  variety  of  acoustic  navigation  systems  have  been  developed 
for  underwater  vehicle  use.  They  are  typically  divided  into  long,  short,  and  ultrashort 
baseline  systems.  All  involve  the  use  of  acoustic  beacons  or  receivers  whose  positions 
must  be  known  to  an  accuracy  somewhat  better  than  the  desired  vehicle  localization 
accuracy  (Tuohy  93).  Unfortunately,  most  acoustic  navigation  systems  require  major 
expeditions  for  their  accurate  set-up  and  periodic  maintenance.  This  makes  them 
expensive,  and  in  many  ways  reduces  the  level  of  autonomy  achievable  by  an  AUV.  Also, 
acoustic  methods  are  affected  by  changes  in  the  speed  of  sound  in  the  ocean  and  suffer  from 
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refraction  and  multipath  propagation  problems  in  restricted  shallow  water  coastal  and  ice- 
covered  areas.  (Tuohy  93) 

There  are  various  alternative  submerged  navigation  methods  not  dependent  upon  the 
aid  of  external  signals.  Charge  Coupled  Device  cameras,  laser  scanning,  or  variations  in 
the  earth’s  magnetic  field  can  aid  in  determining  position  (Bergem  93).  Position  can  also 
be  estimated  by  the  double  integration  of  acceleration  as  sensed  by  an  IMU. 

Doppler  sonar  or  correlation  velocity  log  sensors  can  be  utilized  to  determine  speed 
through  the  water  or  over  the  ground.  Doppler  velocity  logs  utilize  the  physics  of 
frequency  shifts  in  the  sound  waves  of  sources  and  receivers  with  relative  radial  motion.  A 
critical  assumption  for  two-way  transmission  in  the  ocean  is  that  the  sound  scatterers, 
(small  particles  and  plankton)  uniformly  populate  the  environment,  and  at  the  average 
move  at  the  same  horizontal  velocity  as  the  water.  Correlation  velocity  logs,  on  the  other 
hand,  use  reflections  from  the  sea  bottom,  even  at  great  depths,  and  on-board  sensor  arrays 
to  detect  forward  and  lateral  motion  occurring  between  sonar  pings.  (Gordon  96) 

Doppler  technology  has  been  redesigned  as  the  Acoustic  Doppler  Current  Profiler 
(ADCP).  The  ADCP  measures  water  velocity  more  accurately,  and  allows  measurement 
in  range  cells  over  a  depth  profile.  Throughout  the  1980’s,  ADCPs  were  further  improved 
by  production  of  self-contained,  vessel-mounted,  and  direct-reading  models,  and  by  the 
addition  of  broadband  capability  in  1991.  Broadband  ADCPs  take  advantage  of  having 
typically  100  times  as  much  bandwidth  for  measuring  velocity  as  the  original,  narrow- 
bandwidth  models,  reducing  variance  nearly  100  times.  (Gordon  96) 


16 


Broadband  Doppler  processing  computes  the  phase  change  of  propagation  time  delay. 
Since  longer  propagation  times  provide  greater  accuracy,  but  incur  phase  changes  beyond 
360°,  a  mathematical  autocorrelation  function  resolves  ambiguity  and  allows  transmission 
of  a  series  of  coded  pulses  within  a  single  long  pulse.  Multiple  beams  are  utilized  to  obtain 
velocity  in  three  dimensions,  under  the  assumption  of  uniform  currents  across  layers  of 
constant  depth.  Non-homogenous  current  layers  produce  large  velocity  errors.  (Gordon 
96) 

ADCP  single-ping  random  or  short-term  error  may  range  from  just  a  few  mm/s  to  as 
much  as  0.5  m/s,  depending  on  internal  factors  such  as  frequency,  depth  cell  size,  number 
of  pings  averaged  together,  and  beam  geometry.  Since  this  random  error  is  uncorrelated 
from  ping  to  ping,  the  standard  deviation  of  the  velocity  error  can  be  reduced  by  the  square 
root  of  the  number  of  pings  through  averaging.  Although  averaging  can  greatly  reduce  the 
relatively  large,  single-ping  error,  at  a  certain  point  it  fails  to  improve  on  overall  error  as 
the  random  error  becomes  smaller  than  the  bias.  (Gordon  96) 

The  bias  is  typically  less  than  10  mm/s  and  depends  on  factors  such  as  temperature, 
mean  current  speed,  signal/noise  ratio,  and  beam  geometry.  It  is  not  yet  possible  to 
measure  ADCP  bias  and  calibrate  or  remove  it  in  post-processing.  External  error  factors 
include  turbulence,  internal  waves,  and  ADCP  motion,  and  can  dominate  internal  errors. 
While  the  technology  behind  the  ADCP  is  impressive  and  bears  serious  consideration  for 
future  small  AUV  navigation  development,  the  combination  of  relative  affordability  and 
unpredictable  bias  make  it  a  les  attractive  option  for  the  SANS  application.  (Gordon  96) 
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For  covert  missions,  an  AUV  may  not  be  able  to  refer  to  external  signals  while 
submerged.  In  this  case,  the  system  must  rely  on  some  sort  of  dead  reckoning.  Modem 
dead  reckoning  systems  typically  use  magnetic  or  gyroscopic  heading  sensors,  and  a 
bottom  ot  water-locked  velocity  sensor  (Grose  92).  The  presence  of  an  ocean  current  will 
add  a  velocity  component  to  the  vehicle  which  is  not  detected  by  a  water  speed  sensor.  In 
the  vicinity  of  the  shore,  ocean  currents  can  exceed  two  knots  (Tuohy  93).  Using  dead 
reckoning  with  currents  which  are  relatively  large  in  relation  to  the  typical  4-6  knot  speed 
of  an  AUV  can  produce  extremely  inaccurate  results  (Tuohy  93).  This  inaccuracy 
represents  the  central  challenge  of  AUV  submerged  dead  reckoning  navigation. 

There  are  many  techniques  for  measuring  acceleration  and  angular  rates.  These  include 
using  ring  laser  and  fiber  optic  gyros,  rotating  mass  gyros,  vibratory  rate  sensors,  and  high 
performance  IMUs.  Inertial  grade  IMUs  typically  contain  three  angular  rate  sensors,  three 
precision  linear  accelerometers  and  a  three-axis  magnetometer.  The  acceleration 
measurements  required  by  an  INS  can  be  made  by  several  types  of  IMUs.  All  of  these 
sensors  are  subject  to  drift  errors  which  relentlessly  increase  with  time.  High  quality 
sensors  are  subject  to  less  drift,  but  can  cost  up  to  $100,000  (Tuohy  93),  making  them 
unattractive  for  small  AUVs. 

McKeon  (92)  proposes  a  combination  of  GPS  and  INS  to  allow  an  AUV  to  determine 
position  information.  While  submerged,  the  AUV  uses  a  low-cost  inertial  navigation 
system.  However,  when  on  the  surface,  the  vehicle  has  access  to  GPS  information.  GPS/ 
INS  information  could  be  combined  with  Kalman  filter  techniques  to  reduce  errors  during 
the  next  dive  sequence  as  simulated  in  Nagengast  (92)  and  demonstrated  in  McGhee  (95). 
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The  system  described  in  McGhee  (95)  senses  linear  accelerations  and  angular  rates  with 
respective  sensors  and  processes  the  data  in  a  twelve  state  Kalman  filter,  resulting  in  an 
estimated  position.  A  mechanical  water  speed  sensor  and  a  magnetic  compass  are  added 
to  complement  acceleration  and  angular  rate  data  and  further  enhance  navigation  accuracy. 
The  twelve  states  can  be  divided  into  seven  continuous-time  states  (three  Euler  angles,  two 
horizontal  velocities,  two  horizontal  positions),  two  discrete-time  states  derived  from  the 
DGPS  fixes  (estimated  east  and  north  current),  and  three  angular  rate  sensor  bias  estimates, 
(subtracted  from  the  ouqjut  of  these  sensors).  The  DGPS  fixes  occur  aperiodically 
whenever  the  vehicle  surfaces  and  is  able  to  acquire  a  sufficient  number  of  satellites. 
(Bachmann  96) 

F.  NAVIGATION  FILTER  THEORY 

The  inherent  sensor  measurement  errors  plaguing  inertial  measurement  systems  may 
be  partially  compensated  for,  but  never  eliminated.  Drift  is  the  tendency  of  bias  errors  in 
the  angular  rate  sensors  of  the  inertial  platform  to  cause  relentlessly  increasing  orientation 
measurement  errors.  The  single  integration  of  a  bias-ridden  angular  rate  signal  will  cause 
a  steady  build-up  of  error  over  time.  This  leads  to  an  incorrect  estimation  of  the  body 
orientation  relative  to  the  earth-fixed  coordinate  system  and  a  corresponding  body  position 
estimate  error.  Angular  rate  sensor  biases  typically  change  unpredictably  over  time, 
making  a  simple,  complete  compensation  impossible.  (Frey  96) 

Standard  inertial  navigation  procedures  utilize  fix  updates  if  an  alternative  method  of 
determining  instantaneous  orientation  exists.  Drift  is  compensated  for  by  periodic 
adjustments  of  the  inertial  system  to  the  external  reference,  returning  the  bias  error 


19 


accumulation  to  zero.  Short  fix  intervals  then  result  in  relatively  insignificant  bias  errors. 
Higher  quality  angular  rate  sensors  typically  have  lower  bias  errors  and  correspondingly 
longer  fix  intervals.  (Frey  96) 

Linear  acceleration  sensor  drift  errors  are  compounded  by  the  double  integration  of  the 
linear  acceleration  measurements  to  obtain  position  data.  This  results  in  a  position  estimate 
in  error  proportional  to  time-squared,  rather  than  simply  time.  This  error  may  be  similarly 
compensated.  However,  given  the  same  sensor  quality,  the  fix  interval  needed  to  maintain 
comparable  accuracy  will  be  much  shorter  than  that  required  for  the  angular  rate  sensor  bias 
compensation  alone.  (Frey  96) 

Discrete  low  pass  filter  theory  provides  a  method  for  obtaining  a  rate  bias  estimate. 
Such  filters  may  be  represented  by  a  signal-flow  graph  (SFG),  which  is  a  simplified  version 
of  a  block  diagram.  The  SFG  was  introduced  by  S.  J.  Mason  for  the  cause-and-effect 
representation  of  linear  systems  that  are  modeled  by  algebraic  equations  (Kuo  95).  A  SFG 
may  be  defined  as  a  graphical  means  of  portraying  the  input-output  relationships  between 
the  variables  of  a  set  of  linear  algebraic  equations,  or  simply 

Eq(2.1) 

output  =  ^^gain  x  input 

Corresponding  block  and  signal  flow  graph  diagrams  for  a  single  input  discrete  low  pass 
filter  are  shown  in  Figures  1  and  2  below. 
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Figure  1:  Discrete  Low  Pass  Filter  Block  Diagram 


approximated  by 


In  this  diagram,  p~^  stands  for  the  time  domain  integration  operator,  and  tau  is  the 
relaxation  time  constant.  Directly  from  Figure  2, 


Xj(t  +  A0  =  x^(t)  +  x(t)At 


Eq  (2.2) 


or 

Eq(2.3) 

ij  ^  .  input  -  old  output .  ^ 
new  output  =  old  output  +  — - - - - — At 

This  is  the  classic  relationship  describing  a  low  pass  filter  (McGhee  96).  Rewritten, 
Equation  2.3  becomes  Eq  (2.4) 


{input  —  xAt))At 
x^(t  +  AO  =  - 


Which  can  also  be  written 
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Eq(2.5) 


+  AO 


jCj(0  +  input - Xj(0 — 


and,  finally 


AO 


;cj(/  +  AO  =  1 - \+ input 


At 


Eq  (2.6) 


or,  in  more  common  terminology,  and  the  terms  used  in  the  SANS  code 

Eq(2.7) 

new  output  =  outputWeight  x  old  output  +  input  x  sampleWeight 

The  above  general  result  can  be  applied  to  the  SANS  system  for  rate  sensor  bias 

estimation.  In  this  case,  the  signal  used  for  attitude  estimation  is  the  raw  rate  sensor  reading 

with  the  estimated  bias  subtracted.  An  alternative  formulation  is  to  add  the  negative  of  the 


bias  to  the  sensor  reading.  This  formulation  is  derived  similarly,  and  is  implemented  in  the 


SANS  code  as. 


Eq  (2.8) 

new  negative  bias  =  bias  Weight  x  old  negative  bias  -  input  x  sampleWeight 
In  this  form,  the  bias  estimation  integrator  is  initialized  to  a  negative  average  value  and  the 
bias  is  then  added  to  the  sensor  input. 


G.  SUMMARY 

Many  approaches  to  the  problem  of  AUV  navigation  have  been  devised.  New  ones 
are  still  emerging  and  technological  improvements  are  improving  current  approaches. 
Choices  range  from  simple  dead  reckoning,  to  systems  which  use  acoustic  information 
from  floating  or  stationary  transponders,  to  complex  systems  which  use  sophisticated  IMUs 
and  GPS  receivers  combined  with  Kalman  filtering  techniques.  Most  of  the  described 
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approaches  can  only  be  used  in  very  specialized  applications.  Most  are  also  limited  by 
dependence  on  previously  deployed  external  means  and  by  some  requirement  to  actively 
exchange  data  with  those  means.  The  preferred  method  of  many  developers  is  the  acoustic 
approach.  However,  most  of  these  systems  have  a  higher  degree  of  complexity  and 
dependence  on  external  means  than  the  system  implemented  in  McGhee  (95). 

It  can  be  seen  that  high  accuracy  and  other  design  goals  for  an  inertial  navigation 
system  are  achievable.  But  clearly,  the  cost  increases  rapidly  with  the  degree  of 
sophistication  and  the  desired  precision.  From  this  point  of  view  the  NPS  Phoenix  AUV, 
described  in  Healey  (94),  together  with  the  SANS  navigation  system  developed  by 
Bachmann  (95),  McGhee  (95),  Steven  (96),  and  Walker  (96),  promises  to  provide  a  very 
effective  means  for  achievement  of  clandestine  missions  in  shallow  water  by  a  small  AUV. 

The  remainder  of  this  thesis  continues  an  ongoing  experimental  study  pertaining  to  the 
development  of  the  SANS  system  and  associated  problems.  The  current  system  under 
evaluation  is  of  small  physical  size  and  relatively  low  cost.  The  IMU  selected  is 
representative  and  has  limited  accuracy,  so  additional  water-speed  and  magnetic  heading 
information  is  required.  Accelerometers  are  used  mainly  to  derive  low  frequency  attitude 
information,  and  are  not  utilized  for  velocity  or  position  estimation  for  periods  of  more  than 
a  few  seconds. 

Previous  research  on  the  prototype  SANS  has  produced  test  results  and  qualitative 
error  estimates  which  indicate  that  submerged  navigation  accuracy  comparable  to  GPS 
surface  navigation  is  attainable  (Bachmann  95).  The  research  goal  of  this  thesis  is  to  refine 
the  hardware  and  software  configuration  to  allow  more  accurate  submerged  navigation,  and 
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to  develop  the  SANS  into  a  self  contained  system  capable  of  being  internally  or  externally 
attached  to  any  AUV,  delivering  regular,  accurate,  real-time  position  updates. 
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III.  SYSTEM  HARDWARE  CONFIGURATION 


A.  INTRODUCTION 

Bachmann  (95)  describes  the  initial  prototype  in  the  ongoing  development  of  the 
SANS.  Walker  (96)  redesigned  the  original  prototype  to  consolidate  components  in  one 
integrated  system.  In  addition,  he  presented  an  evaluation  summary  of  the  original 
prototype  hardware,  with  particular  emphasis  on  the  noise  characteristics  of  the  Systron- 
Donner  MotionPak  IMU,  which  is  retained  in  the  SANS. 

Figure  3  presents  a  block  diagram  for  the  hardware  making  up  the  redesigned  SANS. 
Figure  4  presents  a  photograph  of  the  SANS  components  fully  assembled  into  their  testing 
configuration.  The  project  box  in  which  the  components  are  currently  mounted  is  an 
interim  solution.  A  more  permanent,  water-tight,  streamlined  housing  is  currently  in 
development. 

This  configuration  is  significantly  different  from  the  previous  prototype  presented  in 
Bachmann  (95).  The  SANS  components  are  no  longer  separated;  all  components  are 
physically  located  in  one  self-contained  package.  When  joined  with  its  accompanying 
power  source  (a  12  VDC  battery),  the  complete  system  can  now  be  strapped-down  to  a  tilt 
table  or  inserted  into  a  towfish  for  at-sea  testing.  In  its  current  configuration,  the  SANS  has 
its  processor  and  GPS/DGPS  components  “on-board,”  thus  no  longer  requiring  the  transfer 
of  sensor  data  via  modem  to  an  external  processor  or  GPS/DGPS  receiver.  (Bachmann  95, 
Walker  96) 

The  SANS  processor  is  linked  with  an  external  processor  via  a  DOS  TCP/IP  network 
connection  to  allow  for  human  monitoring  and  interaction  during  the  course  of  an 
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Figure  3:  Redesigned  SANS  Hardware  Configuration  (Walker  96) 


experiment.  This  external  processor’s  only  function  is  to  maintain  a  remote  control 
session  with  the  SANS  processor  and  receive  its  attitude  and  position  updates.  Unlike  the 
original  SANS  proof  of  concept  design  presented  in  Bachmann  (95),  the  SANS  now 
maintains  the  capability  to  on-board  process  its  own  data  and  interface  with  any  other 
higher-level  processor  via  a  network.  This  capability  will  directly  enable  smooth 
incorporation  of  SANS  into  the  Phoenix  architecture.  This  chapter  will  summarize  the 
hardware  component  capabilities  realized  in  Walker  (96). 
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Project  Box  Cover 


DGPS  Antenna  (tuned  466.7625  MHz) 


Figure  4:  SANS  Hardware  Configuration  (Walker  96) 
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B.  HARDWARE  DESCRIPTION 


1.  Computer 

The  on-board  processor  is  an  Extremely  Small  Package  (E.S.P.)  Cyrix  486SLC  DX2 
50  MHz  computer,  pictured  in  Figure  5.  It  is  specifically  designed  to  offer  off-the-shelf 
PC-compatible  solutions  in  space  and/or  power  constrained  environments.  This  particular 
E.S.P  computer  possesses  a  total  of  eight  modules  which  perform  various  system  tasks. 
Together,  the  processor  and  its  accompanying  modules  provide  a  small,  low-power  system 
with  system  performance  comparable  to  a  standard,  desk-top  type  system.  (MAXUS  95, 
Walker  96) 


Figure  5:  E,S.P.  486SLC  DX2  50  MHz  Computer  (Walker  96) 


The  CPU  Module  provides  the  processing  capability,  the  interface  for  a  standard 
keyboard,  the  Flash  PROM  containing  the  system  BIOS,  and  memory  and  bus  controller 
logic.  The  DC-DC  Power  Module  provides  for  all  the  system  power  requirements  up  to  a 
maximum  35W  total  output.  It  accepts  an  unregulated  12  V  DC  and  provides  the  required 
+5,  +12,  -12,  and  -28  V  DC  to  power  various  system  components  and  optional  peripherals 


(i.e.,  an  external  floppy  and  hard  drive,  as  is  used  in  the  tilt-table  test  configuration). 
(Maxus  95,  Walker  96) 

The  VGA  Adapter  Module  provides  the  interface  to  operate  an  external  VGA  monitor. 
A  PC  I/O  Module  provides  for  two  Serial  ports  and  one  parallel  I/O  port  It  also  provides 
two  type-III  PCMCIA  sockets  which  conform  to  PCMCIA  Release  2.01  standard.  These 
two  ports  can  be  used  for  a  variety  of  compatible  devices  (i.e.,  Ethernet  Adapter,  Modem, 
GPS  Receiver,  etc.).  This  module  was  included  in  the  current  design  to  provide  additional 
secondary  storage  in  the  form  of  PCMCIA  SRAM  cards,  as  well  as  to  enable  possible 
future  expansion.  An  Ethernet  Module  provides  the  SANS  with  an  external  ethemet 
interface.  (Maxus  95,  Walker  96) 

The  Analog  to  Digital  (A/D)  Module  provides  8  differential  or  16  single-ended  input 
channels  at  12-bit  resolution.  In  its  current  configuration,  the  A/D  module  samples  only  8 
of  the  available  16  single-ended  channels.  It  features  a  single-channel  maximum  sampling 
rate  of  333  KHz,  and  an  input  range  from  +/-  1.25mV  to  +/-10V  (MAXUS  95).  The  A/D 
module  provides  a  34-pin  external  connector  (J3)  to  which  developers  can  connect  their 
input  signals.  (Walker  96) 

The  DRAM  Module  provides  for  high-speed  (70ns)  memory  storage  available  in  2, 4, 
6,  8,or  16MB  capacities  (MAXUS  95).  This  module  is  to  the  E.S.P.  as  a  hard  disk  is  to  a 
standard  desk-top  PC.  (Walker  96) 

2.  Inertial  Measuring  Unit 

The  inertial  navigation  component  of  the  SANS  is  provided  by  a  Systron-Donner 
Model  MP-GCCCQAAB-100  “MotionPak”  inertial  sensing  unit,  pictured  in  Figure  6.  This 
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self-contained  unit  provides  analog  measurements  in  three  orthogonal  axes  of  both  linear 
acceleration  and  angular  velocity.  It  consists  of  a  cluster  of  three  accelerometers  and  three 
“Gyrochip”  angular  rate  sensors.  (Walker  96) 


Figure  6:  Systron-Donner  Inertial  Measuring  Unit  (Bachmann  95) 


3.  GPS/DGPS  Receiver  Pair 

The  GPS/DGPS  receiver  used  is  the  ONCORE  8-channel  receiver  which  incorporates 
an  imbedded  DGPS  capability  (Oncore  95).  The  receiver  is  capable  of  tracking  up  to  eight 
satellites  simultaneously.  It  can  provide  position  accuracy  of  better  than  25  meters 
Spherical  Error  Probable  (SEP)  without  Selective  Availability  (SA),  and  100  meters  (SEP) 
with  SA.  Typical  Time-To-First-Fix  is  18  seconds  with  a  typical  reacquisition  time  of  2.5 
seconds  (Oncore  95).  This  receiver  meets  or  exceeds  the  capabilities  of  the  receiver 
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described  in  Norton  (94),  which,  under  normal  operating  conditions,  met  the  accuracy  and 
time  requirements  of  the  SANS  project  Norton  (94)  also  demonstrated  that  a  receiver  with 
these  qualities  will  perform  well  when  using  an  antenna  that  is  located  on  or  near  the  sea 
surface,-  as  is  necessary  during  a  clandestine  mission.  Figure  7  shows  the  ONCORE  GPS/ 
DGPS  receiver  used  in  the  SANS  project  (Walker  96) 


Figure  7:  ONCORE  GPS/DGPS  Receiver  (Walker  96) 

4.  Compass 

The  compass  used  in  the  SANS  project  is  a  Precision  Navigation  model  TCM2 
Electronic  Compass  Module.  This  compass  does  not  employ  the  mechanical  gimbal 
technology  utilized  in  the  compass  described  in  Bachmann  (95),  but  rather  employs  a  three- 
axis  magnetometer  and  a  high-performance  two-axis  tilt  sensor  in  a  small  form-factor 
(TCM2  95).  The  TCM2  compass  is  capable  of  providing  readings  of  pitch,  roll,  and 
surrounding  magnetic  field  strength  in  addition  to  heading.  The  TCM2  provides  greater 
accuracy  by  calibrating  (performed  by  the  user)  for  distortion  fields  in  all  tilt  orientations, 
providing  an  alarm  when  local  magnetic  anomalies  are  present,  and  giving  out-of-range 
warnings  when  the  unit  is  being  tilted  too  far  (TCM2  95).  (Walker  96) 
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5.  Other  Compoirents 

The  water  speed  sensor  and  the  depth  sensor  are  those  described  in  Bachmann  (95)  and 
therefore  are  not  depicted  in  Figure  5.  The  GPS  antenna  shown  in  Figure  5  is  an  active 
antenna,  which  was  selected  for  its  performance  and  low  profile.  Because  the  E.S.P. 
Ethernet  module’s  output  media  type  is  AUI,  a  standard  AUI-to-BNC  media  converter  is 
employed  to  allow  the  use  of  durable  RG-58  coax  cable  to  span  the  roughly  100m  distance 
required  while  pulling  the  towfish  behind  a  towing  vessel.  The  GPS/DGPS  Interface  box 
is  nothing  more  than  an  adapter  to  interface  the  GPS  receiver  signal  with  the  serial  port  of 
the  E.S.P.  computer.  (Walker  96) 

Based  on  the  analysis  given  in  Walker  (96),  the  2-pole  anti-aliasing  Bessel  filters  used 
in  Bachmann  (95)  were  replaced  with  new  low-harmonic  distortion  filters.  These  come 
factory  tuned  to  a  user-specified  comer  frequency  of  10  Hz,  require  no  external  components 
or  adjustments,  and  operate  with  a  dynamic  input  voltage  range  from  non-critical  +/-  5V  to 
+/-18V  power  supplies  (Frequency  Devices  96).  To  implement  these  filters  into  the  SANS, 
a  double-sided  printed  circuit  board  was  designed  and  machined  to  receive  all  six  filter 
DIPS,  as  well  as  three  quad  op-amp  LM324  DIPs  configured  as  voltage-followers  to 
provide  input  and  output  circuit  protection.  (Walker  96) 

To  provide  for  the  requisite  +/-15  VDC,  a  DATEL  model  BWR-15/330-D12  DC-DC 
Converter  is  used  to  convert  the  unregulated  12  VDC  battery  input  into  regulated  4-/- 15 
VDC  needed  to  power  the  low-pass  filter  circuits  and  the  IMU.  This  converter  features 
over-current  and  short-circuit  protection,  a  compact  form-factor,  and  high  reliability  at  a 
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minimum  efficiency  of  82%.  It  employs  switching  regulator  technology,  which  minimizes 
heat  generation  and  current  usage.  (DATEL  95,  Walker  96) 

Physically  connecting  the  IMU,  Low-pass  Filters/DC-DC  Converter  PCB,  the  Analog- 
Digital  Converter,  input  power,  water  speed  sensor,  and  depth  sensor,  is  a  25-strand  flat 
ribbon  cable.  This  type  of  cable  was  chosen  to  allow  all  system  components  to  be  easily 
interconnected.  (Walker  96) 

C.  SUMMARY 

The  SANS  design  described  in  this  chapter  is  significantly  different  from  that 
described  in  Bachmann  (95).  The  processing  capability,  along  with  the  GPS/DGPS 
receiver,  is  now  on-board  the  SANS,  making  it  completely  self-contained.  The  only 
external  link  is  a  DOS  ethemet  environment  to  a  remote  PC  utilized  for  test  monitoring 
purposes.  The  IMU  sensor  data,  after  low-pass  filtering,  along  with  water  speed  and  depth 
data,  are  converted  from  analog  to  digital  form,  with  12-bit  resolution,  and  then  passed  to 
the  processor.  GPS  data  is  passed  separately  to  the  processor,  which  computes  updated 
attitude  and  position  information  to  be  exported  over  an  ethemet  socket.  The  hardware  for 
this  version  of  the  SANS  was  chosen  to  comply  as  far  as  possible  with  the  requirements  set 
forth  in  Kwak  (93).  Though  there  are  many  possible  choices  of  hardware  for  each  of  the 
components  in  Figure  4,  trade-offs  between  accuracy,  size,  power  requirements,  and  cost 
have  been  considered.  As  further  advances  in  miniaturization  are  made,  accuracy  will 
continue  to  increase  while  price  and  size  decrease,  thus  making  it  easier  to  meet  the 
challenges  of  the  SANS  baseline  requirements.  The  next  chapter  of  this  thesis  will  describe 
the  software  which  supports  this  hardware  configuration. 


33 


I  :'23 

I 


34 


IV.  SOFTWARE  DEVELOPMENT 


A.  INTRODUCTION 

The  puipose  of  the  SANS  software  is  to  control  some  of  the  individual  hardware 
components,  to  control  input/output  interface  communications  between  the  components,  to 
assimilate  all  of  the  incoming  data,  and  to  implement  a  twelve  state  navigation  filter.  This 
chapter  will  review  the  software  structure  inherited  from  Bachmann  (95)  and  Walker  (96), 
and  will  concentrate  particularly  on  the  changes  made  to  accommodate  the  greatly 
improved  processing  speed  that  Walker  (96)  made  possible. 

The  code  is  written  in  C++  and  is  designed  for  use  on  an  IBM-compatible  personal 
computer  using  the  Borland  version  3.1  compiler  under  DOS  5.0.  This  project  code  choice 
has  proven  to  complicate  the  integration  of  the  hardware  interfaces.  Additionally,  the  dated 
software  compiler  formats  and  DOS  system  calls  make  the  code  specific  to  this  application 
only  and  increases  the  difficulty  of  troubleshooting  or  implementing  changes.  Although 
most  of  the  code  is  transportable  to  other  C++  compiler  environments,  the  interrupt 
processing  and  input/output  communications  control  uses  obsolete  type  declarations  and 
function  calls  to  the  rapidly  aging  operating  system. 

This  limitation  could  easily  be  resolved  in  future  project  work  in  either  of  two  ways. 
Utilizing  a  Borland  version  5.0  compiler  with  updated  communications  code  would  allow 
continued  use  of  a  traditional  IBM-compatible  environment.  Converting  those  sections  of 
code  to  be  compatible  with  Unix  environment  compilers  could  also  be  implemented  on  the 
PC  under  the  Linux  operating  environment. 
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The  software  design  instantiates  objects  corresponding  either  to  the  individual 
hardware  components  or  to  the  purpose  accomplished,  in  a  straightforward  manner.  The 
class  and  object  relationships  are  shown  in  Figure  8.  All  of  the  concrete  classes  depicted 
are  specifically  instantiated  by  the  class  instance  above  them,  in  descending  chronological 
order  as  the  program  is  initiated.  All  are  instantiated  as  a  single  object,  named  as  shown. 
There  is  no  need  in  this  application  for  extensive  polymorphism.  The  serialPortClass  and 
bufferClass  classes  are  abstract  parent  classes  containing  the  common  definitions  and 
functions  from  which  the  specific  compassPort,  compassBuffer,  gpsPort,  and  gpsBuffer 
classes  inherit.  The  stampedSample  object,  defined  in  the  main  program’s  header  file, 
contains  the  latest  update  of  all  pertinent  navigation  information.  Therefore,  it  is  the  object 
which  is  passed  between  the  class  objects.  Other  objects  which  support  the  calculations  are 
structures  to  hold  such  things  as  position  in  the  various  formats.  For  simplicity,  they  are 
not  shown  in  Figure  8. 

This  architecture  represents  a  substantial  change  from  the  original  design,  while 
retaining  most  of  the  functionality.  As  the  project  evolved,  it  was  determined  that  much  of 
the  flexibility  originally  envisioned  did  not  prove  to  be  necessary.  This  includes  features 
such  as  the  capability  to  instantiate  an  array  of  serial  ports,  or  a  need  for  a  wide  variety  of 
buffers  for  the  data  received  through  the  serial  ports. 

The  above  features  were  included  in  the  original  object  oriented  design  approach,  but 
have  been  streamlined  to  a  more  specific,  less  complicated  structure.  Specifically,  the 
portbank  and  bytebuffer  classes  have  been  removed.  Only  two  serial  ports  are  required,  for 
the  compass  and  gps  interfaces,  respectively.  The  serial  port  code  was  modified,  and  the 
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Main 


Figure  8:  SANS  Code  Classes  and  Objects 
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buffered  serial  port  class  has  been  specialized  to  a  compassPort  class  and  a  gpsPort  class, 
while  retaining  the  same  basic  function.  This  resulted  in  the  compassPort  and  gpsPort 
classes  representing  a  kind  of  serial  port,  similar  to  the  way  the  compBuffer  and  gpsBuffer 
classes  already  were  a  kind  of  bytebuffer  and  continue  to  be  a  kind  of  buffer.  This 
simplified  the  class  membership  hierarchy  and  variable  passing  across  class  lines.  The 
specific  nature  of  the  application  made  efficiency  a  higher  priority  than  general 
applicability. 

Other  improvements  included  the  addition  of  configuration  files  containing  such  data 
as  gain  settings  to  allow  repeated  testing  without  the  necessisity  of  recompiling  after  every 
change.  The  increased  processing  speed  overwhelmed  the  DOS  operating  system’s  ability 
to  print  information  to  the  user’s  screen  in  real  time,  so  an  interval  was  added  that  reduced 
screen  output  to  a  more  usable  human  rate  that  also  reduced  input/oulput  conflicts.  All 
screen  output  and  data  writing  to  files  were  consolidated  to  single  points  to  further  simplify 
exchanges.  And  finally,  some  error  checking  was  added  to  ensure  such  things  as  proper  A/ 
D  converter  channel  initialization. 

B.  SOFTWARE  Fn.TER 

The  purpose  of  the  software  filter  is  to  utilize  IMU,  heading,  and  water-speed 
information  to  implement  an  INS,  and  then  to  integrate  this  with  GPS  information.  This 
results  in  a  single  system  which  can  produce  continuously  accurate  navigational 
information  in  real  time.  The  filter  mitigates  the  effects  of  sensor  inaccuracies  (inherent, 
electronic  noise,  and  transitory),  ocean  current  (the  largest  single  factor  affecting  AUV 
navigation),  dynamic  model  uncertainty,  measurement  errors,  and  calculation  errors. 
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Kalman  filtering  techniques  are  used  to  implement  the  INS  using  DGPS  fixes  as  “error-free 
data”.  This  allows  periodic  reinitialization  of  the  INS  to  correct  accumulated  drift  and 
development  of  error  biases.  All  sensor  data  is  logged  in  raw  form  for  post-mission 
processing.  (Bachmann  96) 

Figure  9  is  a  data  flow  diagram  for  the  SANS  software  filter.  On  this  diagram,  R 
represents  a  rotation  matrix  and  T  is  a  body  rate  to  Euler  rate  transformation  matrix.  Table 
2  gives  the  state  variables  for  the  navigation  filter.  The  twelve  state  variables  include  the 
outputs  of  the  three  integrator  blocks,  the  estimated  current  in  north  and  east  direction 
components,  and  the  bias  estimates  for  the  angular  rate  readings.  (Bachmann  96) 


Euler  Angles 

a>  ,0 

North  &  East  Velocity 

North  &  East  Position 

Apparent  Current 

jc 

Angular  Rate  Bias  Estimates 

Pb  4b  ^b 

TABLE  2:  State  Variables  of  the  Kalman  Filter  (Bachmann  96) 


Ten  of  the  state  components  are  “continuous  time”:  the  three  Euler  angles  ( 4>,  0,  v  ), 
two  horizontal  velocities  y^),  two  horizontal  positions  {x,  ,y^ ),  and  three  angular  rate 

bias  estimates.  Continuous  time  integration  is  approximated  by  numerical  integration, 
making  these  “continuous  time”  components  discrete  time  values  in  the  reality  of  the  digital 
filter.  This  is  necessary  due  to  the  minimum  integration  sampling  time  limitation  of  the 
computer  and  A/D  hardware.  The  apparent  ocean  current  values  {x^,  y^)  are  updated 
aperiodically  as  a  result  of  both  diving  and  wave  action,  which  produce  inherently  discrete 
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gps  fix  information.  This'discrete  event  dynamic  system  is  well  suited  to  application  of 
Kalman  filter  theory  to  obtain  optimal  time- varying  values  for  the  gain  matrices  in 
Figure  9.  However,  at  the  time  of  writing  this  thesis,  there  are  inadequate  statistics  on 
DGPS  noise  and  AUV  maneuvering  as  needed  by  this  approach.  Therefore,  bandwidth 
and  steady-state  error  considerations  were  used  to  compute  initial  constant  gains 
(Bachmann  95,  McGhee  95),  which  were  subsequently  adjusted  based  on  the  results  of 
experimental  studies.  (Bachmann  96) 

One  area  for  future  project  work  involves  obtaining  the  necessary  statistical  data 
needed  for  refinement  of  the  aperiodic,  gps  update  portion  of  the  filter.  The  optimum  reset 
weight  for  application  to  the  final  integrator  block  could  then  be  determined.  Additionally, 
application  of  the  gps  fix  interval  (1/At)  just  prior  to  is  under  consideration  for  removal. 

The  principal  difference  between  the  current  filter  and  that  described  in  Bachmann 
(95)  regards  the  point  in  the  filter  process  at  which  the  apparent  current  error  correction  is 
made.  The  previous  filter  added  the  apparent  current  to  the  water  speed.  The  difference 
between  this  value  and  the  estimated  north  and  east  velocities  was  input  to  the  north  and 
east  accelerations  with  a  gain  Kj, .  Poor  initial  sea  test  results  in  Bachmann  (95)  indicated 
this  approach  was  possibly  underdamped  or  even  unstable.  The  present  approach  is  to 
apply  the  apparent  current  as  feedback  to  the  output  of  the  third  integrator  block,  prior  to 
input  to  the  final,  position  integrator.  (Bachmann  96) 

The  continuous  state  portion  of  Figure  9  shows  that  the  Euler  angle  and  linear  velocity 
outputs  are  fed  back  to  the  corresponding  integrator  inputs.  Thus,  with  diagonal  gain 
matrices  K^,  /sTj ,  and  ATj ,  each  of  these  integrators  is  in  fact  a  low  pass  filter  for  its 
respective  inputs  (Bachmann  96).  Figure  10  isolates  one  feedback  loop  to  help  illustrate 
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this  relationship.  The  integrator  block  is  shown  using  s-domain  (Laplace  transform) 
notation.  This  approach  prevents  unlimited  state  estimate  growth  caused  by  unmodeled 
bias  errors  in  state  derivative  inputs  to  the  integrators  (Bachmann  96).  Complementary, 
low  frequency  information  from  an  independent  source  (accelerometers)  is  also  furnished 
to  each  integrator  to  correct  for  long-term  decay  of  the  state  estimates  resulting  from  this 
feedback  (McGhee  95).  The  low  frequency  information  sources  include  attitude  estimates 
from  accelerations  sensed  by  the  accelerometers  the  magnetic  compass 

readings  (Y,),  and  water  speed  {u„).  (Bachmann  96) 


Figure  10:  Complementary  Filter  Feedback  Loop  for  Euler  Angle  Estimation 

The  IMU  acceleration  readings  require  correction  in  addition  to  filtering.  The 
accelerometer  data  is  utilized  as  an  inclinometer,  to  determine  how  much  of  the  specific 
force  felt  along  each  axis  is  due  to  gravity.  Computed  gravity  is  then  subtracted  from 
specific  force  readings  of  the  accelerometers  (x,  y,  z),  to  transform  them  into 
accelerations,  prior  to  rotation  into  earth-fixed  coordinate  values  (x^  ,  y,  ,  z;).  (Bachmann 
96) 
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The  rate  sensor  input  in  Figure  10  is  added  to  accelerometer  attitude  estimates  after  the 
gain  matrix  is  applied.  This  signal  already  has  the  estimated  bias  removed  utilizing  the  low 
pass  filter  methodology  derived  in  Chapter  n  and  resulting  in  Equation  2.7.  New  biases  are 
calculated  on  each  filter  cycle  by  the  calculateBiasCorrections  function  of  the  insClass, 
and  are  applied  to  new  navigational  state  information  in  the  applyBiasCorrections  function. 
Filter  response  to  example  and  real  world  inputs  will  be  discussed  in  detail  in  Chapter  VI, 
System  Testing. 

C.  IMPLEMENTATION  DESCRIPTION 

Figure  1 1  shows  the  revised  data  flow  between  software  objects.  The  tasks  performed 

by  the  SANS  software  can  be  divided  into  two  basic  categories.  The  primary  tasks  are 
related  to  calculating  the  current  position  and  other  navigational  state  information.  This 
includes  processing  incoming  GPS  data,  IMU  data,  water-speed,  and  heading  information, 
and  integrating  all  of  this  information  through  the  navigational  filter  to  obtain  a  fix.  These 
tasks  are  performed  by  the  gpsClass,  insClass,  and  Navigator  software  objects  respectively. 
The  secondary  tasks  involve  hardware  interfacing,  communications,  data  filtering  and  unit 
conversion.  These  basic  but  crucial  tasks  are  handled  by  the  Sampler,  Buffer,  compBuffer, 
gpsBuffer,  A2D,  Serial  Port,  compassPort,  and  gpsPort  software  objects.  The  main 
program  serves  to  drive  the  other  objects  by  continually  querying  the  navigator  for  position 
updates  and  performs  output  to  the  user  screen  and  data  files  from  a  single  location.  Real 
time  navigation  source  code  is  provided  in  Appendix  A.  Supporting  serial  communication 
and  other  administrative  function  code  is  provided  in  Appendix  B.  The  following  summary 
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Figure  11:  SANS  Data  Flow  Between  Software  Objects 
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of  the  source  code  is  presented  bottom-up  to  illustrate  construction  of  the  navigation  state 
from  the  individual  data  elements.  (Bachmann  95) 

1.  Compass  Data 

The  compassClass  contains  the  code  and  member  objects  which  implement  reception 
of  compass  messages  in  a  design  similar  to,  with  the  exception  of  specific  hardware  details, 
the  gpsClass.  Private  member  compassPort  instantiates  a  kind  of  serialPortClass  object  to 
allow  data  communication  on  COM2.  CompassPort  in  turn  has  private  member 
compBufferClass  which  provides  a  kind  of  bufferClass  structure  for  temporary  storage  of 
incoming  compass  messages.  Figure  12  illustrates  the  compBufferClass  and 
gpsBufferClass  data  stractures.  The  compassClass  therefore  contains  code  to 
communicate  with  the  serial  port,  as  well  as  to  check  the  “checksum”  and  header  of  each 
compass  message  received.  The  samplerClass  object  instantiates  compassClass  object 
“compl”  and  periodically  interrogates  compl  to  empty  the  buffer  of  information. 
(Bachmann  95,  Walker  96) 

2.  GPS  Data 

The  gpsClass,  as  previously  mentioned,  is  similar  in  design  to  the  compassClass,  with 
differences  driven  by  the  different  message  formats,  and  it  utilizes  COMl.  It  obtains  GPS 
position  messages  in  the  Motorola  proprietary  format  (@@Ea).  Before  the  code 
recognizes  a  GPS  message  as  being  valid,  the  message  must  pass  three  conditions;  1)  it 
must  have  a  valid  checksum,  2)  the  fix  must  be  based  on  at  least  4  satellites,  and  3)  the 
differential  bit  in  the  message  must  be  set  (i.e.,  the  fix  must  have  the  differential  correction 
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applied  to  it).  The  navigatorClass  instantiates  gpsClass  object  “gpsl”  and  interrogates  gpsl 
to  empty  its  buffer  directly.  (Bachmann  95,  Walker  96) 


compBuffer  “compData”  /  gpsBuffer  “GPSdata” 


( twice  as  long  as  message  in  each  cas£) 


compBuffer  “headings”  gpsBuffer  “messages” 

last  current 

block 

Figure  12:  Buffer  Data  Structures 
3.  Inertial  Sensor  Data 

Inertial  sensor  data  passes  through  the  new  filter  circuit  board.  From  there,  it  is  input 
directly  to  the  A/D  converter  module  in  the  processor. 

a.  AID 

The  A/D  module  came  with  demonstration  C  source  code  provided  by  the  unit 
manufacturer.  Walker  (96)  modified  the  demo  code  and  converted  it  to  C++  for  the  SANS 
application.  The  a2dClass  provides  all  of  the  requisite  software  operation  for  the  A/D 
module  in  the  E.S.P.  computer,  which  is  completely  controlled  fnrough  software.  Control 
is  maintained  through  the  manipulation  of  the  AO  Control  Register  and  the  A/D  Status 


Register.  These  registers  are  manipulated  by  writing  to  and  reading  from  specific  memory 
addresses.  The  a2dClass  is  designed  with  some  degree  of  user  flexibility.  For  instance,  the 
user  can  choose  between  one  of  two  base  addresses.  (Walker  96) 

•  The  SANS  software  only  uses  a  few  of  the  member  functions  in  accomplishing 
its  mission.  Those  member  functions  not  directly  utilized  in  this  particular  application  are 
useful  for  troubleshooting,  or  allow  a  variety  of  options  for  specific  applications.  The 
following  general  discussion  explains  how  the  A/D  module  works  in  the  SANS  application. 
(Walker  96) 

The  A/D  provides  12  bits  of  resolution,  or  2*^  =  4096  discrete  quantization 
levels.  The  A/D  module  may  be  employed  in  differential  mode  or  single-ended  mode.  The 
SANS  application  employs  the  A2D  in  the  single-ended  mode  of  operation.  The  A2D 
samples  the  dual-ended  swing  of  the  MU  sensor  signals,  and  represents  these  voltages  as 
a  digital  value  in  the  range  0  -  4095.  A  general  A/D  conversion  table  is  provided  as  Table 
3  to  further  illustrate  how  the  sensor  voltages  are  mapped  over  to  their  digital  equivalents. 
(Walker  96) 


Sensor  DC  Voltage 

Converted  Equivalent 

+10  Volts 

4095 

+5  Volts 

3071 

0  Volts 

2047 

-5  Volts 

1023 

-10  Volts 

0 

TABLE  3:  A2D  DC-to-Digital  Conversion  Mapping  (Walker  96) 
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When  an  a2dClass  object  is  instantiated,  the  class  constructor  sets  several  default 
data  member  values,  and  then  reads  the  A/D  configuration  file  A2D.cfg.  This 
configuration  file  provides  simple  user  update  of  A/D  module  operation  without 
recompiling  the  source  code.  The  constructor  initializes  the  system  addresses,  then 
initializes  the  A/D  hardware  using  those  variables  that  were  loaded  upon  reading  the 
configuration  file.  The  a2dClass  object  is  instantiated  by  the  samplerClass  object  as 
“a2dr’.  It  is  a  private  data  member  of  the  samplerClass.  (Walker  96) 

The  A/D  module  is  set  into  operation  by  a  call  to  the  samplerClass  function 
initSampler().  It  utilizes  a2dClass  member  functions  to  program  the  sequencer  and  tell  it 
which  channels  to  sample  and  in  what  order,  resets  the  A/D  First-In-First-Out  (FIFO)  to 
enable  it  to  receive  data,  and  then  toggles  the  trigger  bit  in  the  A/D  Control  Register  from 
a  low  to  a  high,  which  starts  the  A/D  into  operation.  (Walker  96) 

4.  Sampler 

The  samplerClass  object  prepares  raw  IMU,  heading,  and  water  speed  data  for  use  by 
the  INS.  This  preparation  includes  simple  filtering,  unit  conversion,  and  time  stamping. 
Figure  13  provides  a  summary  of  the  principal  class  members  and  functions,  with 
psuedocode  descriptions  of  the  principal  methods.  The  samplerClass  interface  consists  of 
a  single  method  (getSample)  which  controls  the  data  formatting  and  returns  a  formatted 
sample  if  valid  raw  data  is  available,  and  a  negative  response  otherwise.  (Bachmann  95) 

Figure  14  provides  an  illustration  of  the  process  of  obtaining  samples  from  the  A/D. 
During  SANS  operation,  the  samplerClass  member  function  readSamples()  is  called 
repeatedly  to  retrieve  inertial  data  from  the  A/D  FIFO.  It  first  checks  to  ensure  that  the 
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Figure  13:  sampIerClass  Summary 
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FIFO  emptied  into  double  array: 

x-accel 

y-accel 

z-accel 

x-ang 

y-ang 

z-ang 

waterspeed 

depth 

0  1  2  . 

timeCounter  +  1  /  sample 
timeCounter  *  sample  period  =  deltaT 


Figure  14:  samplerClass  Data  Flow 


FIFO  is  not  FULL.  If  the  FIFO  ever  gets  filled  without  being  immediately  emptied,  data 
will  continue  to  push  into  the  FIFO.  There  is  no  room  for  this  additional  data  and  all 
information  from  that  point  on  will  be  lost.  Preventing  the  FIFO  from  overflowing  is 
critical  for  proper  SANS  operation.  If  this  check  is  ever  true,  the  SANS  software  has  been 
rewritten  to  reinitialize  the  a2d  and  continue  to  navigate.  One  full  FIFO  plus  the  data 
received  in  the  time  since  the  overflow  will  be  discarded.  This  will  result  in  a  very  short 
period  of  lost  data  with  a  minimal  impact  on  navigation  accuracy. 

To  prevent  FIFO  overflow,  one  need  only  be  mindful  of  the  rate  at  which  the  A/D  is 
sampling  its  inputs  and  be  sure  the  A/D  FIFO  is  emptied  at  the  same  rate  or  faster.  If  the 
FIFO  does  have  data  in  it,  this  data  is  emptied  from  the  FIFO  and  stored  in  a  doubly- 
subscripted  array  with  8  rows  and  1000  columns  to  coincide  with  storing  up  to  1000,  8 
channel  samples  of  sensor  data.  This  type  of  data  stmcture  is  used  to  temporarily  store  the 
data  to  enable  access  to  a  history  of  samples.  Figure  15  presents  a  model  of  this  array. 
(Walker  96) 
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Figure  15:  Model  of  the  A2D  Sample  Array  (Walker  96) 
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The  first  action  taken  by  the  Sampler  when  a  packet  is  received  is  to  time  stamp  it. 
Since  the  time  difference  between  the  eight  samples  contained  in  a  single  message  packet 
is  relatively  small,  the  Sampler  object  then  respectively  averages  the  eight  corresponding 
data  variables  contained  in  a  packet.  As  the  samples  are  emptied  from  the  FIFO,  the 
variable  “timeCounter”  is  incremented  once  for  every  8  samples.  This  variable  is  then 
multiplied  by  the  sample  period  to  calculate  the  “deltaT”,  or  the  time  between  adjacent 
samples.  The  samplerClass  code  then  averages  over  all  the  samples  received  since  the  last 
sample  was  taken  from  this  array.  The  averaged  measurements  which  result  represent  a 
simple  low-pass  filtering  of  the  samples.  This  has  the  effect  of  filtering  out  small 
fluctuations  in  the  data.  (Bachmann  95,  Walker  96) 

The  integers  contained  in  a  sample  are  digital  measurements  of  analog  voltages  output 
by  the  SANS  sensors.  Once  these  eight  filtered  measurements  are  obtained  they  are 
converted  from  voltages  to  units  which  are  usable  by  the  INS  object  (i.e.,  feet  and  radians). 
Finally,  each  of  the  measurements  is  checked  to  ensure  that  it  is  within  the  limits  of  the 
sensor  from  which  it  came.  If  any  values  fall  outside  the  capabilities  of  the  sensor  from 
which  it  came,  the  entire  packet  is  considered  invalid  and  discarded.  (Bachmann  95) 

5.  INS 

The  INS  class  implements  the  SANS  inertial  navigation.  It  is  the  most  complex  class 
in  the  software.  It  has  been  changed  very  little  as  the  project  has  evolved.  Figure  16 
provides  a  summary  of  the  principal  member  objects  and  functions  which  constitute  the 
INS  methods.  The  interface  consists  of  three  public  methods.  Each  is  directly  involved  in 
the  implementation  of  the  twelve-state  Kalman  filter.  The  primary  method  (insPosition) 
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combines  all  sensor  information  and  uses  the  Kalman  filter  to  produce  a  dead  reckoning 
position  estimate.  The  other  methods  support  the  primary  method  by  performing  one-time 
or  periodic  operations.  Initialization  of  the  INS  is  performed  by  the  insSetUp  method, 
which  sets  the  INS  posture  at  the  grid  coordinate  origin,  sets  an  initial  heading  and  speed, 
and  marks  the  beginning  of  the  first  integration  intervals.  The  last  public  method  of  the 
class  (correctPosition)  inputs  GPS  information  to  reinitialize  the  INS  position  while 
determining  a  current  and  error  correction  bias.  The  INS  class  instantiates  a  samplerClass 
object  “saml”,  from  which  it  obtains  all  sensor  data  except  for  GPS  position  fixes. 
(Bachmann  95) 

6.  Navigator 

The  navigatorClass  acts  as  coordinator  of  all  navigational  information.  As  such  it 
determines  which  source  is  currently  providing  the  best  information,  converts  various 
position  formats  from  one  format  to  another,  and  instantiates  the  GPS  and  ENS  objects 
“gpsl”  and  “insl”.  Like  the  insClass,  this  portion  of  the  code  has  been  changed  very  little 
as  the  project  has  evolved.  Figure  17  provides  a  sumniary  of  the  class  members  and 
functions  that  provide  the  principal  navigation  methods.  The  interface  to  the  object  is  made 
up  of  two  public  methods.  (Bachmann  95) 

The  main  program  instantiates  navigatorClass  object  “navi”.  The  first  method  of  the 
navigatorClass  (initializeNavigator),  initializes  navi,  preparing  it  to  begin  providing  the 
current  position  upon  request.  This  method  obtains  an  initial  GPS  fix  for  use  as  the  origin 
of  the  grid  used  by  the  INS  object  to  specify  positions,  and  calls  the  initialization  method 
of  the  INS.  (Bachmann  95) 
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Figure  17:  Navigation  Class  and  Initialization  Summary 
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The  second  navigator  method  (navPosit)  drives  both  the  GPS  and  INS  objects,  and 
provides  the  navigator’s  best  estimate  of  current  position  in  hours,  minutes,  seconds  and 
milliseconds  of  latitude  and  longitude.  Each  time  the  method  is  invoked,  it  interfaces  with 
the  GPS  -and  INS  objects  to  determine  if  none,  one,  or  both  have  an  updated  estimate  of  the 
current  position.  If  no  update  is  available,  the  navigator  returns  a  negative  reply  indicating 
that  it  can  not  provide  a  position  update.  If  only  INS  information  is  available,  it  is  returned 
as  the  current  estimated  position.  Whenever  GPS  information  is  available,  it  overrides  the 
INS  estimate  of  position.  GPS  information  is  also  passed  to  the  INS  object  as  a  reference 
for  reinitialization  and  error  estimation  purposes.  (Bachmann  95) 

The  navigator  deals  with  three  different  position  formats.  GPS  positions  from  the 
Motorola  receiver  are  initially  obtained  entirely  as  latitude/longitude  in  milliseconds.  INS 
positions  are  expressed  in  x-y  grid  coordinates  based  upon  a  navigator-stored  origin.  GPS 
positions  must  be  converted  to  grid  coordinates  prior  to  utilization  by  the  INS.  The 
positions  produced  by  the  navigator  are  expressed  in  hours,  minutes  and  seconds  of  latitude 
and  longitude.  A  total  of  four  methods  are  used  to  convert  from  one  format  to  another. 
Figure  1 8  illustrates  uses  and  conversions  of  the  different  position  formats.  (Bachmann  95) 

7.  Communication  Objects 

The  bufferClass  and  serialPortClass  objects  are  abstract  parent  classes  from  which 
specific  instances  are  instantiated  for  the  compassClass  and  gpsClass,  respectively.  As 
such,  they  contain  the  common  class  members  and  functions  to  support  the  routine  but 
essential  tasks  of  serial  port  communication  and  buffering  received  data. 
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USER 


Figure  18:  Navigation  Position  Format  Utilization  (Bachmann  95) 

D.  SUMMARY 

The  SANS  software  is  designed  to  produce  continuously  accurate  navigational 
information  in  real  time.  While  submerged,  IMU,  heading  and  water-speed  information  are 
processed  by  the  SANS  Inertial  Navigation  System  (INS)  to  produce  a  dead  reckoning 
position  estimation.  This  is  integrated  with  DGPS  information  obtained  during  aperiodic 
surfacings  using  Kalman  filtering  techniques.  The  DGPS  information  is  used  to  reset  the 
position  of  the  INS.  It  is  also  used  to  generate  an  apparent  current  vector  to  correct  future 
INS  position  estimates.  (Bachmann  95) 
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The  software  was  implemented  using  object  oriented  paradigms.  It  was  written  in 
Borland  version  3. 1 ,  for  use  on  an  IBM-compatible  processor.  The  primary  tasks  of  the 

software  are  estimation  of  current  position  and  communication.  The  former  is  handled  by 
the  Navigator,  Sampler,  a2d.  Compass,  ENS,  and  GPS  classes.  The  later  is  accomplished  by 
the  bufferClass,  compBufferClass,  gpsBufferClass,  serialPortClass,  compassPortClass, 
and  gpsPortClass  objects.  (Bachmann  95) 

The  next  chapter  of  this  thesis  will  present  the  testing  methodology  and  results  for  the 
tilt-table  tests  of  the  operational  code. 
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V.  SYSTEM  TESTING 


A.  INTRODUCTION 

This  chapter  presents  both  the  testing  methodology  and  the  experimental  results  of  the 
tilt-table  testing  used  to  determine  the  functionality  and  accuracy  of  the  SANS  attitude 
estimation.  These  tests  focus  on  the  operational  C-h-  code,  on  determination  of  optimal 
gain  settings  for  the  attitude  portion  of  the  navigation  filter,  and  on  evaluation  of  the 
hardware  accuracy  and  noise  characteristics  in  a  controlled  environment.  Factors  which 
control  attitude  response  include  the  ATj  gain  value,  the  bias  weight  (biasWght),  sample 

weight  (sampleWght),  and  the  x  and  y  axis  accelerometer  scale  factors. 

As  a  reminder  from  Chapter  H,  the  rate  sensor  input  in  Figure  1 1  has  the  estimated  bias 
removed  utilizing  the  low  pass  filter  methodology  resulting  in  Equation  2.7.  Further 
background  on  low  pass  filter  bias  response  is  provided  below  in  order  to  show  the 
reasoning  behind  the  testing  methodology  and  to  help  explain  the  results. 

B.  LOW  PASS  FILTER  BIAS  RESPONSE 

Applying  Mason’s  formula  to  the  signal-flow  graph  of  Figure  2  from  Chapter  2,  in  the 
s  (Laplace  transform)  domain  gives  the  transfer  function  of  a  low  pass  filter  as 

(EqS.l) 

J_ 

_  T5  _  1  _  ^^^^actual  _  Ljoutput]  _  Y(s) 

14-T5  l+xs  commanded  L{input}  U{s) 

A  typical  tilt-table  test  of  the  attitude  and  angular  rate  sensors  involves  a  step  input  of 

a  constant  roll  rate  to  a  commanded  roll  angle,  for  example,  10  degrees  per  second  to  an 
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angle  of  45  degrees,  resulting  in  a  4.5  second  input.  The  filter  bias  estimation  response  can 
be  determined  from  x(t)  =  u(t),  leading,  in  the  s  domain,  to  X(s)  =  U(s)  =  1  /  s,  and 


1 

.Y(s)  =  Gis)X(s)  =  7 

1  +  X5 
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5(1  +T5) 
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(Eq  5.2) 
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(Eq  5.3) 


and 

y(t)  =  -  (Eq5.4) 

T 

which  represents  the  bias  filter  output  slope.  Thus,  the  simplified  response  of  the  bias 
estimation  to  the  initial  roll  input  is  an  exponential  rise  beginning  at  the  instant  the  input  is 
initiated.  After  one  time  constant  (t),  63  percent  of  the  input  value  has  been  reached.  The 
output  value  gradually  approaches  the  limit  of  the  input  as  time  continues.  This  is 
graphically  represented  in  Figure  19. 


Figure  19:  Bias  Filter  Response  to  a  Roll  Rate  Step  Input  of  10°/sec 
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The  developed  testing'  procedure  for  the  SANS  allows  approximately  20  seconds  of 
initial  stabilization  time  for  the  components  and  filter  to  “steady  out”.  This  was  followed 
by  an  initial  roll  input,  a  similar  stabilization  period  after  the  platform  had  reached  the 
commanded  angle,  and  then  return  to  the  zero  position  at  the  same  rate.  Typically,  two  of 
these  cycles  were  performed  under  each  testing  condition. 

To  shift  a  unit  step  to  start  at  20  seconds 
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(Eq  5.5) 


The  example  input  pulse  of  4.5  seconds  can  be  written 

x(t)  =  10(u(t-20)-u{t-24.5)) 
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for  a  time  constant  of  1000  seconds.  This  produces  the  first  part  of  the  response  illustrated 
in  Figure  20. 


Figure  20:  Estimated  Short  Term  Bias  Response  to  a  45  Roll  Completed  in  4.5 
Seconds 


For  times  equal  to  or  greater  than  24.5, 
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(Eq  5.10) 

(r-24.5)  (r-24.5) 

1000  ==  0.045 e 


This  result  is  shown  in  Figure  21  on  a  longer  scale  to  illustrate  the  gradual  correction 
over  time. 


Figure  21:  Estimated  Long  Term  Bias  Response  to  a  45°  Roll  Completed  in  4.5 
Seconds 

Taken  together.  Figures  20  and  21  illustrate  that  the  bias  response  of  a  low  pass  filter 
to  a  time-shifted  step  roll  input  is  a  rapid  rise  to  the  calculated  value,  followed  over  the 
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length  of  the  relaxation  time  constant  by  a  gradual  correction  to  zero.  Combining  this 
response  with  the  complementary  filter  design,  incorporated  as  depicted  in  Figure  11, 
results  in  the  time  domain  filter  response  including  a  time  lag  effect  which  barely  sees 
minor  transients.  The  initial  response  to  a  step  change  in  attitude  comes  almost  entirely 
from  the  angular  rate  sensors.  Over  time,  input  from  the  accelerometers  takes  over  and 
compensates  exactly  for  the  decay  of  the  rate  sensors.  The  nature  of  this  response 
influenced  development  of  the  testing  methodology  and  is  directly  reflected  in  the 
following  testing  results. 

C.  FILTER  TESTING  METHODOLOGY 

The  tilt-table  testing  methodology  has  evolved  through  Bachmann  (95)  and  Walker 

(96).  Although  basically  unchanged  from  the  method  used  in  Walker  (96),  it  is  presented 
here  in  a  standardized,  sequential  order  with  extensive  background  for  the  first  time.  It  is 
also  presented  at  Appendix  D  in  a  checklist  format.  The  testing  methodology  is  designed 
to  separate  the  complementary  effects  of  the  filter  and  treat  them  individually  before 
evaluating  the  entire  filter  process. 

The  SANS  is  mounted  to  the  tilt-table  described  in  (Bachmann  96)  for  a  series  of  pitch 
and  roll  tests.  If  the  unit  is  carefully  leveled  prior  to  testing,  the  actual  commanded  attitudes 
are  extremely  accurate  in  reference  to  real-world  pitch  and  roll  angles.  Relative  angle 
excursions  are  always  extremely  accurate  on  the  tilt-table.  The  amount  of  the  actual  angle 
excursion  is  the  important  value  for  the  testing.  In  other  words,  a  valid  45  degree  pitch  from 
a  beginning  baseline  of  2  degrees  to  47  degrees,  for  example,  is  a  successful  test  for  the 
IMU.  Once  calibrated  and  installed  in  Phoenix,  the  SANS  becomes  the  reference  for 
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attitude  determination.  That  is,  if  roll  and  pitch  SANS  outputs  are  zero,  then  this  defines 
level  orientation  for  Phoenix. 

The  general  procedure  is  to  allow  a  15  to  20  second  period  for  the  sensors  to  initialize 
and  stabilize  after  the  filter  code  begins  execution.  This  is  followed  by  a  pitch  or  roll 
excursion  to  45  degrees  at  various  commanded  rates  (consistent  during  each  individual 
run).  The  unit  is  then  tilted  back  to  the  zero  position,  followed  by  a  roll  excursion  in  the 
opposite  direction,  and  then  finally  back  again  to  zero.  Each  movement  is  followed  by  the 
stabilization  period  to  allow  observation  of  filter  effects.  Those  cases  where  the  excursions 
were  both  in  the  same  direction  reflected  physical  limitations  as  to  how  the  SANS  box 
could  be  mounted  on  the  tilt-table.  Maximum  tilt  rate  was  90  degrees  per  second,  but  tests 
were  normally  conducted  at  either  10  or  45  degrees  per  second.  These  conditions  are  much 
more  severe  than  those  encountered  by  the  SANS  in  the  real  world,  with  the  possible 
exception  of  surfaced  operations  in  a  very  heavy  sea  state,  and  therefore  represent  worst 
case  performance  for  the  filter. 

In  order  to  determine  the  rate  sensor  bias  value,  is  set  to  zero  to  prevent 
accelerometer  inputs  fi-om  effecting  the  results.  Therefore,  only  the  high  frequency  angular 
rate  and  bias  get  to  the  first  integrator.  Any  errors  in  attitude  can  then  be  attributed  to  the 
bias  and  scale  factor.  The  appropriate  initial  angular  rate  scale  factor  (qScale  for  pitch,  etc.) 
is  then  determined  by  taking  the  commanded  tilt-table  angles  as  truth.  The  scale  factor 
adjusts  the  output  of  the  IMU  to  the  actual  tilt  results.  Starting  with  a  baseline  of  1.0,  it  is 
possible  to  continuously  apply  the  ratio  of  indicated  and  actual  angles  to  the  current  setting 
in  order  to  scale  it  to  a  proper  value.  For  example,  if  the  SANS  says  the  unit  pitched  to  41° 


when  the  actual  pitch  was  45°,  the  new  scale  factor  is  increased  to  45/41  multiplied  by  the 
old  scale  factor. 

The  initial  bias  weight  (biasWght)  is  chosen  through  a  combination  of  project 
experience  and  filter  theory  considerations.  Extensive  simulation  and  tilt-table 
experiments  can  then  refine  the  proper  values  prior  to  at-sea  testing. 

After  setting  the  gain  weight  to  some  value  other  than  zero,  multiple  test  runs  can 
refine  the  proper  settings.  The  accelerometer  scale  factors  are  then  adjusted  in  the  same 
manner  as  the  angular  rate  scale  factors  if  indications  show  that  the  combined  inputs  result 
in  inaccurate  angle  excursions.  A  complete  tuning  of  one  axis  may  take  an  extensive  set  of 
alternating  adjustments  to  the  various  factors,  as  illustrated  in  the  testing  results  provided. 

D.  IMU  TEST  RESULTS 

The  testing  results  included  here  utilized  the  current  hardware  configuration,  along 
with  the  original  code  from  Bachmann  (95)  only  slightly  modified  to  improve  input/output 
rates.  This  resulted  in  update  rates  of  approximately  18  Hz.  The  complete  code  revision 
described  in  this  thesis  resulted  in  an  increased  update  rate  of  40  Hz,  making  the  filter  real¬ 
time  capable  for  the  first  time.  That  update  rate  unfortunately  overwhelms  the  internal  data 
storage  of  the  SANS  in  the  current  configuration,  so  further  testing  will  have  to  either  be 
done  at  reduced  rates  or  be  conducted  after  new,  larger  storage  cards  (now  available)  have 
been  obtained. 

Figure  22  shows  the  initial  pitch  test  run.  Both  values  are  set  to  0.0,  isolating  the 
angle-rate  input  from  the  accelerometer  input.  Pitch  was  at  a  rate  of  10  degrees  per  second. 
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The  qScale  value  had  already  been  adjusted  to  4.02  to  reflect  45°  of  pitch.  When  compared 
to  previous  project  results  (see  Walker  (96)  and  Bachmann  (95)),  the  faster  update  rate 
significantly  reduced  initial  overshoot  of  the  final  pitch  angles.  The  stabilization  periods 
following  each  pitch  show  that  the  effects  of  the  filter  cancel  in  that  the  initial  slight 
overshoots  gradually  return  to  the  proper  value,  regardless  of  pitch  direction,  as  expected 
from  the  earlier  explanation.  In  fact,  for  the  pitch  which  is  initiated  at  approximately  20 
seconds,  if  no  other  pitch  excursions  occurred,  the  angle  value  would  become  essentially 
45°  by  1020  seconds  (20  +  x).  The  stabilization  period  is  only  a  small  fraction  of  the  time 
constant,  and  the  bias  is  subtracted  from  each  new  sample.  Thus,  the  accumulated  bias 
from  the  excursion  is  only  partially  corrected  for,  with  a  slope  in  the  direction  of  the 
“correct”  value. 

Figure  23  shows  a  second  pitch  test  with  all  values  unchanged  with  the  exception  of  x, 
which  increased  from  1000  to  5000.  Ideally,  the  filter  should  be  initialized  for  a  period  of 
one  time  constant,  however,  the  shorter  stabilization  periods  here  are  sufficient  to 
demonstrate  filter  behavior.  The  stabilization  periods  of  Figure  23  show  a  flatter  slope  than 
those  of  Figure  22.  This  reduced  slope  shows  that  increasing  x  minimizes  the  accumulated 
rate  bias. 

Turning  to  the  roll  axis.  Figure  24  shows  the  initial  roll  test.  The  time  constant  x  has 
been  reset  to  1000  seconds.  The  roll  rate  is  still  10  degrees  per  second  and  the  initial  scale 
factor  (pScale)  was  set  to  be  4.01.  The  nearly  identical  scale  factors  show  the  IMU  to  be 
very  consistent  between  axis.  Otherwise,  the  roll  results  are  similar  to  the  pitch  results. 
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Figure  22:  Initial  Pitch  Test,  =  0.0,  x  =  1000,  qScale  =  4.02, 10°/sec 


The  roll  rate  was  increased  to  40  degrees  per  second  for  the  second  roll  test.  This 
becomes  obvious  with  the  more  widely  separated  fix  dots  on  the  graph  in  Figure  25.  Since 
the  update  may  occur  at  any  point  during  it’s  cycle  (worst  case  immediately  before  the 
commanded  angle  is  reached),  more  overshoot  is  possible,  and  in  fact  occurs.  This  leads 
to  a  more  pronounced  return  effect  during  the  stabilization  periods. 

The  third  roll  test,  shown  at  Figure  26,  has  identical  settings  to  the  previous  test  with 
the  exception  of  /ifi ,  which  has  been  set  to  0.01  to  allow  an  accelerometer  effect  to  return. 
This  is  what  causes  the  wander  in  roll  angle  seen  in  the  initial  stabilization  period.  This 
effect  is  also  present  in  the  stabilization  following  the  initial  roll,  but  is  less  pronounced 
after  the  return  to  the  zero  position  as  the  time  grows  closer  to  the  initial  time  constant. 
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Increasing  Ki  to  0.05  and  reducing  x  to  200  produces  the  results  of  Figure  27.  These 
stabilization  periods  are  characterized  by  more  aggressive  corrections  to  the  “proper” 
angle.  Both  Figure  26  and  27  show  the  importance  of  increasing  the  filter  update  rate  from 
the  18  Hz  rate  shown  to  the  40  Hz  rate  achieved  in  this  thesis  to  prevent  undershoot  and 
overshoot  due  to  sampling  effects.  The  results  of  Figure  27  are  essentially  duplicated, 
although  at  a  reduced  roll  rate  of  10  degrees  per  second,  in  Figure  28. 


0  20  40  60  80  100  120 

Figure  27:  Roll  Test:  =  0.05,  x  =  200,  pScale  =  4.01, 40  °/sec 


The  following  roll  test.  Figure  29,  shows  the  effect  of  varying  the  accelerometer  scale 
factor  (yAccelScale)  from  1.34  to  1.405.  The  stabilization  periods  are  flatter  with  respect 
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to  the  “correct”  angle  and  the  accelerometer  effects  less  pronounced  when  compared  to 
Figure  28. 

Figure  30  returns  the  time  constant  to  1000  seconds  while  also  doubling  It  was 
determined  at  this  point  that  the  yAccelScale  value  had  been  adjusted  too  high.  Prior  to  the 
next  roll  test  (Figure  31),  it  was  adjusted  by  (45/48)  *  1.405  since  the  unit  computed  an 
initial  roll  of  48°  vice  45.  Figure  31  shows  a  flatter  response,  but  there  is  still  some 
overshoot.  The  pScale  was  adjusted  again  for  the  test  shown  in  Figure  32  by  the  amount 
(45/46)  *  4.01.  Finally,  the  yAccelScale  was  adjusted  once  again  by  (45/44)  *  1.317  to 
produce  the  output  in  Figure  33.  This  sequence  clearly  illustrates  the  alternating,  gradually 


Figure  30:  Roll  Test:  =  0.1,  x  =  1000, 10°/sec,  yAccelScale  =  1.405 
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finer  tuning  approach  which  must  be  taken  in  order  to  tune  a  filter  of  this  complexity. 
Figure  33  clearly  has  the  least  overshoot/undershoot  and  the  flattest  stabilization  periods 
while  exhibiting  a  proper  correction  tendency  before  the  next  input  is  encountered. 

Figures  34  and  35  are  provided  to  illustrate  filter  response  at  the  more  radical  rates  of 
457sec  and  907sec.  Although  there  is  slightly  more  overshoot,  as  expected,  even  at  these 
extremes,  the  filter  behaves  predictably  and  well  within  acceptable  accuracy  for  the 
Phoenix  or  other  small  scale  portable  navigation  applications. 


0  20  40  60  80 


0 


Figure  33:  Roll  Test:  =  0.1,  t  =  1000,  yAccelScale  =  1.347,  li 


Figure  34:  Roll  Test:  same  as  previous,  with  45°/  sec  vice  10 


Figure  35:  Roll  Test:  same  as  previous,  with  90°/sec  vice  45 
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E.  SUMMARY 

This  chapter  has  provided  a  methodology  for  dynamic  tilt-table  testing  with  rationale 
and  illustrative  experimental  results.  Taken  together,  the  results  graphically  show  that  the 
SANS  design,  code  architecture,  and  filter  implementation  are  performing  as  expected. 
Additionally,  while  room  for  some  improvement  remains,  the  sensor/filter  combination  is 
easily  accurate  enough  to  meet  both  the  Phoenix  AUV  and  other  potential  small  scale 
portable  navigation  applications.  It  is  important  in  reviewing  the  results  presented  to 
remember  that  these  testing  conditions  are  much  more  severe  than  are  likely  to  be 
encountered  in  actual  SANS  operation  except  when  surfaced  in  significant  sea  states. 
Other  independent  testing  of  the  SANS  approach  (Renault  96)  suggests  that  attitude 
estimation  to  an  accuracy  of  a  few  tenths  of  degrees  should  be  realized  in  normal  operating 
conditions. 

Addition  of  a  math  coprocessor  to  the  E.S.P  CPU  module  has  increased  performance 
dramatically  and  decreased  the  undersampling  seen,  as  expected  by  Walker  (96). 
Accompanying  code  revisions  have  resulted  in  a  legitimate  real-time  navigation  filter 
which  is  expected  to  improve  accuracy  even  further.  The  final  chapter  of  this  thesis  wUl 
review  conclusions  reached  and  recommendations  for  future  project  work. 
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VI.  CONCLUSIONS  AND  RECOMMENDATIONS  FOR  FUTURE 

WORK 


A.  CONCLUSIONS 

The  research  topics  addressed  by  this  thesis  were:  1)  evaluate  the  hardware  and 
software  architecture  of  the  SANS,  2)  develop  a  calibration  procedure  for  the  SANS 
navigation  filter,  3)  evaluate  the  specific  performance  of  the  SANS  navigation  filter,  and  4) 
evaluate  the  SANS  hardware  and  software  architecture  for  installation  in  the  Phoenix  AUV. 
Each  incremental  step  in  the  SANS  project  work  has  provided  evolutionary  improvement 
in  capability  and  performance.  Walker  (96)  built  on  the  Bachmann  (95)  hardware 
prototype  and  provided  the  current  hardware  capability.  This  thesis  has  improved  on  the 
code  architecture  of  Bachmann  (95)  to  accommodate  the  greatly  increased  processing 
speeds  resulting  from  the  Walker  (96)  hardware  configuration  and  addition  of  a  math 
coprocessor. 

A  basic  tilt-table  testing  methodology  was  utilized  for  an  overall  evaluation  of  the 
SANS  attitude  estimation  pursuant  to  addressing  the  research  issues.  Combining  the 
procedures  used  in  Walker  (96)  and  Bachmann  (95)  to  produce  a  specific  filter  calibration 
procedure  simultaneously  addressed  all  of  the  topics  in  a  general  manner.  The  results 
showed  that  the  filter  is  working  correctly  and  as  expected  from  the  supporting  theory. 
Furthermore,  the  real-time  capability  now  makes  SANS  a  bonafide  option  as  a  new 
navigation  solution  for  Phoenix  or  alternative  small  scale  portable  navigation  applications. 
The  SANS  project  is  now  poised  for  meaningful  at-sea  trials  to  further  validate  the  recent 


77 


improvements  to  allow  further  development  of  the  linear  velocity  and  position  estimation 
portions  of  the  filter. 

B.  RECOMMENDATIONS  FOR  FUTURE  WORK 

There  remain  many  areas  for  further  research  on  the  SANS  project.  The  next  major 

step  will  be  at-sea  testing  utilizing  a  tow  fish  as  in  Bachmann  (95).  Successful  completion 
of  these  tests  will  make  the  SANS  ready  for  adaptation  and  installation  in  Phoenix  if  it  is 
chosen  as  the  navigation  solution.  Incorporation  into  Phoenix  is  expected  to  be  very 
straightforward.  The  ethemet  connection  can  be  utilized  to  pass  the  Phoenix  “Officer  of 
the  Deck”  software  module  the  required  navigation  state  elements.  These  elements  are 
currently  stored  at  each  update  and  written  to  a  data  file.  Compatibility  issues  should  be 
limited  to  data  communication  between  SANS  and  the  Phoenix  navigator  software.  In  the 
meantime,  purchase  of  a  larger  PCMCIA  SRAM  card  will  immediately  alleviate  the  data 
storage  problem  encountered  during  laboratory  testing  resulting  from  the  faster  processing 
speeds. 

Consideration  should  be  given  to  updating  the  software  utilized  in  SANS.  Two 
approaches  exist.  The  first  is  to  update  the  DOS/BORLAND  PC  environment  by 
upgrading  to  the  latest  versions.  This  option  will  entail  rewriting  some  of  the  basic  input/ 
ou^ut  system  function  calls.  The  second  option  would  be  a  complete  rewrite  to  make  the 
software  compatible  with  the  final  Linux  or  LonWorks  implementation  option  that  is 
incorporated  into  Phoenix.  Although  more  involved,  this  option  is  attractive  because  it 
prevents  a  proliferation  of  different  operating  systems  within  the  same  architecture. 
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Postprocessing  of  the  navigation  data  file  remains  an  unfinished  area  from  Bachmann 
(95)  and  Walker  (96).  Test  rans  could  be  repeated  multiple  times  to  more  easily  optimize 
the  Kalman  filter  gains.  In  a  related  matter,  the  incorporation  of  the  aperiodic  GPS  updates 
into  the  overall  Kalman  filter  scheme  also  still  remains  to  be  refined.  The  author  hopes  that 
the  results  presented  in  this  thesis  will  prove  to  be  valuable  in  this  ongoing  effort. 
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APPENDIX  A:  Real  Time  Navigation  Source  Code  (C++) 


A.  TOWTYPES.H 

tifndef  _ TOETYPES_H 

#define  _ TOETYPES_H 

#include  "globals .h"  //  Types  used  by  serial  communications  software 

#define  GPSBLOCKSIZE  76  //  Size  of  Motorola  @@Ea  position  message 

#define  PACKETSIZE  133  //  Size  of  packet  received  via  X-modem  protocol 

#define  COMPSIZE  60 

#define  ONE_G  32.2185  //  One  g  in  feet  per  second 

#define  GRAVITY  32.2185  //In  feet  per  second 

#define  TicksToSecs (x)  ((double)  ((10  *  x)  /  182)) 

typedef  char  ONEBYTE; 
typedef  short  TWOBYTE; 
typedef  long  FOURBYTE; 

typedef  unsigned  char  UNSIGNED_ONEBYTE; 
typedef  unsigned  short  UNSIGNED_TWOBYTE; 
typedef  unsigned  long  UNSIGNED_FOURBYTE; 

struct  latLongMilSec  {  //  Holds  lat/long  expressed  in  miliseconds 

long  latitude; 
long  longitude; 

}; 

//  Holds  a  latitude  or  longitude  expressed  in  hours  minutes  and  degrees 
struct  T_GEODETIC  { 

TWOBYTE  degrees; 

UNSIGNED^TWOBYTE  minutes ; 
double  seconds; 

}; 

//  Holds  a  latitude  and  longitude  expressed  as  T_GEODETICs 
struct  latLongPosition  { 

T_GEODETIC  latitude; 

T_GEODETIC  longitude ; 

}; 

struct  grid  {  //  Holds  a  grid  position 

double  x,y,z; 

}; 

struct  matrix  {  //  3  X  3  matrix 

float  element [3] [3]; 

}; 
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//  3  X  1  matrix  or  vector 


struct  vector  { 

float  element[3]; 

); 


//  Oversize  area  to  hold  a  GPS  message 
typedef  BYTE  GPSdata[2  *  GPSBLOCKSIZE] ; 

//  Defines  a  type  for  holding  compass  messages 
typedef.  BYTE  compData[2  *  COMPSIZE] ; 

//  Structure  for  passing  around  various  types  of  INS  information - 
//  The  positions  in  the  sample  field  of  a  stampedSample  structure 
//  sample[0]:  x  acceleration  gnuplot:  2 


//  sample[l]:  y  acceleration  3 

//  sample[2]:  z  acceleration  4 

//  sample[3]:  phi  (roll)  5 

//  sample[4]:  theta  (pitch)  6 

//  sample[5]:  psi  (yaw)  7 

//  sample [6]:  water  speed 

//  sample[7]:  heading 


struct  stampedSample  { 

Boolean. gpsFlag; 

Boolean  insFlag; 
latLongPosition  navLatLong; 
grid  est; 

GPSdata  satPosition; 
float  r awSamp 1 e [ 8 ] ; 
double  sample[ll]; 
double  deltaT; 
float  bias[3]; 
float  current [3]; 

); 

#endif 


//  True  —  GPS  fix  obtained 
//  True  —  INS  fix  obtained 
//  posit  in  hours,  mins,  secs 
//  position  as  estimated  by  the  INS 
//  the  latest  GPS  position 
//  Original  readings  for  post  processing 
//  sampler  converted  sample 
//  delta  of  the  sample 
//  bias  corrections 
//  error  correction  current 
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B.  TOEFISH.CPP 


#include  <stdlib.h> 

#include  <stdio.h> 

#include  <string.h> 

#include  <iostream.h> 

# include  <conio.h> 

# include  <dos.h> 

# include  <time.h> 

#include  " toetypes .h" 

# include  “nav.h" 

extern  compass PortClass  port27  //  so  breakhandler  can  call  destructors 

extern  gpsPortClass  portl;  //to  insure  cleanup  on  program  exit 

int  breakHandler (void) ; 

void  screenSetUp (void) ; 

void  printPosition  (const  latLongPositionSc)  ; 
void  positOut (stampedSample&  posit); 

//  Write  an  INS  packet  and  its  timeStamp  to  the  out Put  file 
void  writeData (const  stampedSampleSc  drPosition,  ofstreamSc)  ; 

//  Write  a  GPS  message  to  the  outPut  file, 
void  writeGpsData  (const  GPSdataS:  satPosition)  ; 

PROGRAM:  Main 

AUTHOR:  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 

DATE:  11  July  1995,  last  modified  January  1997 

FUNCTION:  Drives  the  navigator  and  its  associated  software.  Counts 
the  positions  &  displays  each  to  the  screen.  Exited  only 
when  control  break  (Ctrl  c)  is  entered  at  the  keyboard. 
RETURNS :  0 

CALLED  BY:  none 

CALLS:  initializeNavigator  (nav.h) 

navPosit  (nav.h) 
printPosition 
breakHandler 

int 

main  { ) 

{ 

//  trap  all  breaks  to  release  com  ports 
//  turn  break  checking  on  at  all  times 


ctrlbrk (breakHandler ) ; 
setcbrkd)  ; 

char  dataFile[]  =  "att.dat"; 
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cout  «  "\nWriting  attitude  data  to  “  «  dataFile  «  endl; 

//  Instantiate  the  navigator  (also  private  members  gpsl  &  insl) 


navigatorClass  navi; 


ofstream  attitudeData (dataFile); 


stampedSample  curLoc; 


//  Lat/Long  of  most  recent  fix 


Boolean  fixReceived (FALSE) ; 
int  f ixCount ( 0 ) ; 

float  timeCount ( 0 . 0 ) ; 


//  True  if  a  new  fix  was  received 
//  Count  of  navigation  fixes  received 
//  Counter  for  screen  output 


cerr  «  “ \nlnitializing  .  .  ,"  «  endl; 


navi . initializeNavigator (curLoc) ; 

//  Check  a2d  initialization,  channels  off  if  y-accel  i=  *-32.2 
while  (curLoc.  sample  [2]  <=  -*33.0  II  curLoc.  sample  [2]  >=  -31.5)  { 
cerr  «  “reinitializing  for  a2d  channelization"  «  endl; 
navi . initializeNavigator (curLoc) ; 
navi . navPos it ( curLoc ) ; 


clrscr ( ) ; 
gotoxy (1,6) ; 

cerr  «  "Initialization  Complete!"  «  endl ; 
cout  «  "Initial  Position:"  «  endl; 


//  Print  the  initial  position 

cout  «  "latitude:  "  «  curLoc .navLatLong . latitude .degrees  «  ':' 

«  curLoc . navLatLong . latitude .minutes  << 

«  curLoc . navLatLong . latitude . seconds  «  endl; 
cout  «  "longitude:  "  «  curLoc .navLatLong . longitude . degrees  « 
«  curLoc . navLatLong . longitude .minutes  « 

«  curLoc . navLatLong . longitude . seconds ; 


screenSetUp ( ) ; 

while  (TRUE)  {  //  Attempt  to  get  a  fix  from  the  navigator 

fixReceived  =  navi .navPosit (curLoc) ; 

if  (fixReceived)  {  //  New  fix  received 

//  Save  fix  info  to  the  data  file 
writeData (curLoc,  attitudeData) ; 

//  Print  info  to  screen  at  designated  print  interval 
f ixCount++ ; 

timeCount  +=  curLoc . del taT; 
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if  (timeCount  >=1.0)  { 
gotoxy (9, 11) ; 
cout  «  fixCount  «  endl; 
positOut (curLoc) ; 
timeCount  =  0.0; 

} 

} 

}  * 

} 

/★★*********★*★★*★*★*★★****★*********★★*****★*****★****★*******★******* 
PROGRAM:  printPosition 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Displays  position  to  the  screen 

RETURNS:  void 

CALLED  BY:  main 

CALLS :  none 

********************************************^**************************/ 

void  printPosition  (const  latLongPositionSc  posit) 

{ 

gotoxy (11, 14) ; 

cout  «  posit .  latitude. degrees  «  '  :  •« 

posit .latitude. minutes  «  «  posit . latitude . seconds  «  endl; 

gotoxy (12, 15) ; 

cout  «  posit .  longitude  .degrees  «  ':'« 

posit .longitude. minutes  «  «  posit . longitude . seconds 

«  endl; 


PROGRAM :  breakHandler 

AUTHOR:  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 

DATE:  11  July  1995 

FUNCTION:  Cleans  up  com  ports  upon  program  exit. 

RETURNS :  0 

CALLED  BY:  main 

CALLS:  compass  port  and  gps  port  destructors 

★  *************-**********************************-^***********************/ 

int  breakHandler (void) 

{ 

port2  .  ~coinpassPortClass  ( )  ; 
portl . -gpsPortClass ( ) ; 

raturn  0;  //  keep  the  compiler  happy 


) 
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PROGRAM: 
AUTHOR : 
DATE: 
FUNCTION: 
RETURNS: 
CALLED  BY; 
CALLS  : 


screenSetup 

Eric  Bachmann,  Randy  Walker 
12  May  1996 

Sets  up  the  output  screen 
0 

main 

none 


void  screenSetUp (void) 

{ 

gotoxy (4, 11) ; 
cout  «  "Fix  " ; 

gotoxy (1, 14) ; 

cout  «  "Latitude:  "  «  " \nLongitude : 
gotoxy (1, 17) ; 

cout  «  "Roll:  "  «  "\nPitch: 

gotoxy (1, 25) ; 
cout  «  "deltaT: 

int  col (45) , row(l) ; 

gotoxy (col , row++ ) ; 
cout  «  "x  accel :  " ; 
gotoxy (col , row++ ) ; 
cout  «  "y  accel: 
gotoxy ( col , row++ ) ; 
cout  «  "z  accel: 
gotoxy ( col , row++ ) ; 
cout  «  "phi  dot: 
gotoxy ( col , row++ ) ; 
cout  «  " theta  dot :  " ; 
gotoxy ( col , row++ ) ; 
cout  «  "psi  dot:  "; 
gotoxy ( col , row++ ) ; 
cout  «  "water  speed: 
gotoxy (col, row++) ; 
cout  «  "heading: 

col  =  45; 
row  =  12 ; 

gotoxy ( col , row++ ) ; 
cout  <<  "x: 
gotoxy ( col , row++ ) ; 
cout  «  "y :  " ; 
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gotoxy  (col,  row++)  ; 
cout  «  “ z :  "  ; 

gotoxy { col , row++ ) ; 
cout  «  “phi: 
gotoxy (col , row++ ) ; 
cout  «  "theta: 
gotoxy { col , row++ )  ; 
cout  «  "psi: 

gotoxy (45, 20) ; 

cout  «  "Bias  Values"; 

gotoxy (60,20) ; 

cout  «  "Current  Values"; 


PROGRAM:  posit Out 

AUTHOR:  Eric  Bachmann 


DATE:  21  October  1996 


FUNCTION:  Updates  the  Screen 

RETURNS :  0 

CALLED  BY:  main 
CALLS :  none 


void  positOut (stampedSample&  posit) 

{ 

printPosition (posit .navLatLong) ; 

if  (posit .gpsFlag)  { 
gotoxy (20,11) ; 
cout  «  "GPS"; 

} 

else  { 

gotoxy (20 , 11 ) ; 
cout  «  " 

} 

//  Output  the  bias  values 

for(int  j  =  3;  j  <  6;  j++)  { 

gotoxy (45, j+18) ; 
cout  «  posit .bias [j ] ; 

} 

//  Set  output  precision  and  fixed  format 
//cout .precision (6) ; 

/ / cout . s  e t  f ( ios : : f ixed ) ; 
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//  Display  linear  accelrations  and  angular  rates 
for  (j  =  0;  j  <  8;  j++)  { 

gotoxy (59, j+1) ; 
cout  «  posit . rawSample [j ] ; 

} 

//  Display  time  delta  to  the  screen. 

gotoxy (9 , 25) ; 

cout  «  posit .del taT; 

//  Display  roll  and  pitch 
gotoxy (8, 17) ; 

cout  «  (posit .sample[3]  *  radToDeg) ; 
gotoxy (8, 18) ; 

cout  «  (posit . sample [4]  *  radToDeg); 

//  Display  current  location  and  posture 
for  (j  =  0;  j  <  6;  j++)  { 

gotoxy (52, j+12) ; 
cout  «  posit .sample[j]; 


//  Display  error  current  values 
for  (j  =  0;  j  <  3;  j++)  { 

gotoxy ( 6  0 , j  +2 1 ) ; 
cout  «  posit .current [j]; 


//  Output  the  biases 
for  (j  =  3;  j  <  6;  j++)  { 

gotoxy ( 4  5 , j  + 1 8 ) ; 
cout  «  posit .bias [j ] ; 

) 

) 


PROGRAM:  writeData 

AUTHOR:  Eric  Bachmann,  Dave  Gay 


DATE: 

FUNCTION: 

RETURNS : 
CALLED  BY: 
CALLS: 


11  July  1995 

Writes  the  packet  and  the  time  stamp  contained  in  a  stamped 

sample  to  the  out  put  file  for  post  processing. 

void 

navPosit  (nav.cpp) 

None 
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void  writeData  (const '  stampedSampleSc  drPosition,  ofstreamSc  attitudeData) 

{ 

static  float  elapsedTime ( 0 . 0 ) ; 

elapsedTime  +=  drPosition .deltaT; 

//  Output  attitude  data  to  a  file 
attitudeData 

«  elapsedTime  «  '  ' 

«  drPosition. sample [0]  «  '  ' 

«  “1.0  *  drPosition. sample [1]  «  '  ' 

«  drPosition. sample [2]  «  ‘  ' 

«  (radToDeg  *  drPosition. sample [3 ] )  «  '  * 

«  (radToDeg  *  drPosition. sample [4] )  «  ‘  ' 

«  (radToDeg  *  drPosition. sample [5] )  «  '  ' 

«  drPosition. sample [6]  «  ’  * 

«  (radToDeg  *  drPosition . sample [7 ] )  «  '  ' 

«  drPosition.current [0]  «  *  ' 

«  drPosition.current  [1]  «endl; 

) 


PROGRAM: 

AUTHOR: 

DATE: 

FUNCTION: 

RETURNS : 
CALLED  BY: 
CALLS: 


writeGpsData 

Eric  Bachmann,  Dave  Gay 

11  July  1995 

Writes  a  raw  GPS  message  to  a  binary  output  file  for 

post  processing. 

void 

navPosit  (nav.cpp) 

None 


/* 

void 

navigator :: writeGpsData  (const  GPSdataSc  satPosition) 

{ 

for(  int  j  =0;  j  <  GPSBLOCKSIZE;  j++)  { 

putc (sat Posit ion[j ] ,  rawData) ; 

} 

} 

*/ 

//  end  of  file  toefish.cpp 
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C.  NAV.H 


#ifndef  _NAVIGATOR_H 
#define  __NAVIGATOR_H 
#include  <stdio-h> 

#include  <fstreain.h> 

#include  <iostreain.h> 

#includ'e  <math.h> 

# include  <dos.h> 

# inc lude  " toe types . h " 

# include  "globals.h” 

# inc lude  “gps .h" 

# inc lude  "ins.h" 

/**★★★*★★****★***★*★***★★★*★★*★***************************************** 
CLASS :  navigatorClass 

AUTHOR:  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 

DATE:  11  July  1995,  Modified  January  1997 

FUNCTION:  Combines  GPS  and  INS  information  to  return  the  current 
estimated  position. 


class  navigatorClass  { 
public : 

//  Constructor,  initializes  object  slots 
navigatorClass 0  :  gpsSpeedSum(0 . 0 )  ,  insSpeedSum ( 0 . 0 ) 

{  cerr  «  " \nconstructing  navi"  «  endl;  ); 

--navigatorClass  {)  {}  //Destructor 

//  provides  the  navigator's  best  estimate  of  current  position 
Boolean  navPosit  { stampedSampleSc)  ; 

//  Initialize  the  navigator 

Boolean  initializeNavigator  ( stampedSampleS:)  ; 
void  userInitNav(stampedSample&)  ;  //  Allows  user  to  initialize  nav 

private : 

double  gpsSpeed,  insSpeed,  gpsSpeedSum,  insSpeedSum; 
insClass  insl;  //  ins  object  instance 

gpsClass  gpsl;  //  gps  object  instance 

//  Obtains  system  time  to  utilize  for  origin 
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double  getSysteroTime ( ) ; 


latLongMilSec  origin;  //  lat-long  of  navigational  origin 

//  Returns  the  position  in  Miliseconds 
latLongMilSec  getMilSec (const  GPSdata&) ; 

//  Returns  the  position  in  degrees,  minutes,  seconds  and  milisecs 
latLongMilSec  latLongToMilSec (const  latLongPositionSc) ; 

//  Convert  position  in  milSec  to  degress,  minutes,  seconds  and  milsec 
latLongPosition  milSecToLatLong (const  latLongMilSecSc) ; 

//  Convert  xy  (grid)  position  to  lat  long 
latLongMilSec  gridToMilSec (const  grid&) ; 

//  Converts  lat /long  to  xy  position 
grid  milSecToGrid (const  latLongMilSecSc)  ; 

//  Parses  and  returns  the  time  of  a  GPS  message, 
double  getGpsTime (const  GPSdataSc  rawMessage) ; 

//  Parses  and  returns  the  velocity  in  fps  of  a  GPS  message, 
double  getGpsVelocity (const  GPSdata&  rawMessage); 

■»  . 

; ; 

#endif 
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D,  NAV.CPP 

#include  <signal.h> 
# include  <dos  ..h> 
#include  <tiine.h> 

# include  “nav.h" 


#define  SIGFPE  8  //  Floating  point  exception 

/★*'<t*************^*******************************************^*********^ 

PROGRAM:  navPosit 


AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Provides  the  navigator's  best  estimate  of  current  position. 

Attempts  to  obtain  GPS  and  INS  position  fixes  from  the  gps 
and  ins  objects  and  copies  the  most  accurate  fix  available 
into  the  input  argument  ‘ navPosit ion ' .  Sets  a  return 
flag  to  indicate  whether  a  valid  fix  was  obtained. 

RETURNS:  TRUE,  a  valid  position  fix  is  in  the  variable  ' navPosition ' . 

FALSE,  otherwise. 

CALLED  BY:  towfish.cpp  (main) 

CALLS:  gpsPosition  (gps.h)  gridToMilSec  (nav.h) 

correctPosition  (ins.h)  milSecToGrid  (nav.h) 

insPosition  (ins.h)  milSecToLatLong  (nav.h) 

getMilSec  (nav.h)  writeScriptPosit  (nav.h) 


void  fpeNavPosit ( int  sig) 

{if  (sig  -=  SIGFPE)  cerr  «  "floating  point  error  in  navPosit\n" ; } 

Boolean  navigatorClass  :: navPosit  ( stampedSampleSc  posit) 

{ 

signal  (SIGFPE,  fpeNavPosit); 

latLongMilSec  gpsMilSec;  II  the  latest  GPS  position  in  milseconds 

latLongMilSec  insMilSec;  II  the  latest  INS  position  in  milseconds 

II  Attempt  to  get  the  INS  and  GPS  positions 

posit . insFlag  =  insl . insPosition (posit ) ; 

posit .gpsFlag  =  gpsl .gpsPosition (posit . satPosition) ; 

II  INS  and  GPS  positions  obtained? 
if  (posit . insFlag  &&  posit . gpsFlag)  { 

II  Parse  position  from  GPS  messsage 
gpsMilSec  =  getMilSec (posit . satPosition) ; 

posit. est  =  milSecToGrid (gpsMilSec) ; 
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//  Pass  GPS  position  to  INS  object  for  navigation  corrections, 
insl .correctPosition (posit ,  getGpsTime (posit . satPosit ion) ) ; 

//  Convert  position  in  milisec  to  latitude  and  longitude, 
posit .navLatLong  =  milSecToLatLong (gpsMilSec) ; 

return  TRUE; 

}  ‘ 

else  { 

if  (posit .insFlag)  {  //  Only  INS  position  obtained? 

insMilSec  =  gridToMilSec (posit . est) ; 
posit .navLatLong  =  milSecToLatLong ( insMilSec) ; 
insSpeed  =  posit . sample [ 6] ; 
return  TRUE; 

} 

else  { 

if  (posit .gpsFlag)  {  //  Only  GPS  position  obtained? 

//  Parse  position  from  GPS  messsage 
gpsMilSec  =  getMilSec (posit . satPosition) ; 
posit. est  =  milSecToGrid (gpsMilSec) ; 

//  Pass  GPS  position  to  INS  object  for  navigation  corrections, 
insl . correctPosition (posit ,  getGpsTime (posit . satPosition) ) ; 

//  Convert  position  in  milisec  to  lat/long. 
posit . navLatLong  = 

milSecToLatLong (getMilSec (posit . satPosition) ) ; 
return  TRUE; 

} 

else  { 

return  FALSE;  //No  new  position  available 

} 

} 

} 

} 


PROGRAM:  initial izeNavigator 

AUTHOR:  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 


DATE:  11  July  1995 

FUNCTION:  Obtains  an  initial  GPS  fix  for  use  as  a  navigational  origin 

for  grid  positions  used  by  the  INS  object.  Saves  the  origin 
and  passes  it  to  the  INS  object  in  latLong  form. 


RETURNS :  TRUE 

CALLED  BY:  towfish  (main) 

CALLS:  gps Posit ion  (gps.cpp) 

correctPosition  (ins .cpp) 
writelnsData (nav. cpp) 


wr i t eGpsData ( nav . cpp ) 
ge tMi ISec  ( nav . cpp ) 
mi 1 S e cToGr id  ( na v . cpp ) 
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Boolean  navigatorClass  :  :  initializeNavigator  (stainpedSample&  posit) 

{ 

Boolean  gp s Flag (FALSE) ; 

cerr  «  "Initializing  Navigator."  «  endl; 
cerr  «  "  Initializing  GPS."  «  endl; 

//  Loop  until  an  initial  GPS  fix  is  obtained, 
for  (int  i  =  1  ;  ((i  <  100)  &&  (gpsFlag  ==  FALSE))  ;  i++)  { 

if  (gpsl. gpsPosition (posit .satPosition) )  { 

gpsFlag  =  TRUE; 

} 

else  { 

delay (500) ; 

} 

} 

if  (gpsFlag  ==  FALSE)  { 

cerr  «  "\nWARNING:  UNABLE  TO  OBTAIN  INITIAL  GPS  FIXl  "  «  endl; 
userInitNav{posit ) ; 

} 

else  { 

cerr  «  "  GPS  initialization  complete."  «  endl; 

//  Save  navigational  origin  for  later  grid  position  conversions, 
origin  =  getMilSec (posit . satPosition) ; 

//  Pass  time  of  first  GPS  fix  to  INS  object  initialization  routine, 
insl . insSetUp (getGpsTime (posit . satPosition) ,  posit)  ; 

} 

cerr  «  "Navigator  initialization  complete."  «  endl; 
return  TRUE; 

} 


PROGRAM:  userInitNav 

AUTHOR:  Rick  Roberts 

DATE:  03  November  1996 

FUNCTION:  Allows  user  to  input  current  position  and  initialize 

nav  if  no  gps  fix  is  available,  (ie  for  testing) 

RETURNS:  void 

CALLED  BY:  init ializeNavigator 

CALLS:  getMilSec  (nav. cpp) ,  getSystemTime  (nav.cpp) 

*★★**★*****★★★*★** ★★★********************************************'^*****/ 
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void  navigatorClass:  ruserInitNav{stampedSainpleS:  posit) 

( 

int'  choice; 


cerr  «  " \nEnter  a  0  to  enter  posit  and  continue  without  GPS" 

«  "\nEnter  a  1  to  continue  without  GPS  or  initial  posit,  or" 

«  "\nEnter  a  2  to  exit:  "  «  endl; 
cin  *»  choice; 

if  (choice  ==  0)  { 

cerr  «  "\nEnter  current  position  in  the  following  format:  "  «  endl; 
cerr  «  "Latitude:  (36,  Enter,  35  Enter,  41.5  Enter)"  «  endl; 
cin  »  posit . navLatLong . lat itude . degrees ; 
cin  »  posit .navLatLong . lat itude .minutes ; 
cin  »  posit .navLatLong . latitude . seconds ; 

cerr  «  "Longitude:  (-121,  Enter,  52,  Enter,  30.2,  Enter)"  «  endl; 
cin  >>  posit .navLatLong . longitude .degrees ; 
cin  »  posit .navLatLong. longitude .minutes; 
cin  »  posit . navLatLong . longitude . seconds ; 

} 

else  if  (choice  ==  2)  {  exit(l);  } 


//  Save  nav  origin  for  later  grid  position  conversions 
origin  =  latLongToMilSec (posit .navLatLong); 


//  Pass  system  time  of  initialization  to  ins  object 
insl . insSetUp (getSystemTime ( ) ,  posit) ; 


PROGRAM: 
AUTHOR : 
DATE: 
FUNCTION: 

RETURNS : 
CALLED  BY: 
CALLS: 


latLongToMilSec 
Rick  Roberts 
22  January  1997 

Converts  a  position  expressed  in  latitude  and  longitude 
degrees,  minutes  and  seconds  to  mili  seconds  Sc  returns  it. 
latLongMilSec 
userInitNav 
none 


latLongMilSec  navigatorClass: : latLongToMilSec (const  latLongPositionS: 
latLong) 

{ 

latLongMilSec  milSec; 

double  degrees,  minutes,  seconds; 

mi ISec . latitude  =  ( latLong . latitude . degrees  *  DEGREES_TO_MSECS )  + 

( latLong.  latitude  .minutes  MINS_TO_MSECS )  + 

( latLong . latitude . seconds  *  1000.0); 


milSec  .  longitude  =  ( latLong .  longitude  .  degrees  DEGREES_TO_MSECS )  + 
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( la-tLong .  longitude  .minutes  *  MINS_TO_MSECS )  + 

( latLong . longitude , seconds  *  1000.0); 

return  milSec; 

) 

/★*★**★****★★*★***★★★*******★*★★★★★*★**★*****★★*★*★*★****★***★★*★★**★*** 
PROGRAM :  g e t Sy s  t  emT ime 

AUTHOR :  Rick  Robe r t s 

DATE:  03  November  1996 

FUNCTION:  Obtains  system  time  to  utilize  for  origin. 

RETURNS:  double  (origin  time  in  seconds) 

CALLED  BY:  userInitNav 

CALLS:  dos  time  function 

double  navigatorClass : : getSystemTime ( ) 

{ 

dostime_t*  sysTime;  //  pointer  to  dos  time  structure 

_dos_gettime  (sysTime)  ; 

return  double ( (sysTime ->hour  *  3600.0)  +  (sysTime->minute  *  60.0) 

+  ( sysTime ->second) ) ; 

) 

/★★★★★*★★★***★*****★★**★******★****★*★****★★*****★★*★★**★★★★**★★★★**★*:<:★ 

PROGRAM:  getMilSec 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Extracts  a  position  in  miliseconds  from  a  Motorola  (@@Ba) 
position  contained  in  the  input  argument  ' rawMessage ‘ . 
RETURNS:  The  latitude  and  longitude  in  miliseconds. 

CALLED  BY:  navPosit  (nav.cpp) 

initializ eNa vi ga t or  { na v . cpp ) 

CALLS :  none . 

★  ★★★★★★★***********************************'******************‘*‘*********/ 

latLongMilSec  navigatorClass :: getMilSec ( const  GPSdata&  rawMessage) 

{ 

FOURBYTE  temps4byte; 
latLongMilSec  position ; 

temps4byte  =  rawMessage [ 15 ] ; 

temps4byte  =  ( temps4byte«8)  +  rawMessage  [16]  ; 

temps4byte  =  ( temps4byte<<8)  +  rawMessage [ 17 ] ; 

temps4byte  =  ( temps4byte«8)  +  rawMessage[18]; 

position. latitude  =  temps4byte; 
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temps4byte 

teinps4byte 

teinps4byte 

tGinps4byte 


'=  rawMessage [ 19 ]  ; 

=  (teinps4byte«8 )  +  rawMessage  [20]  ; 
=  (temps4byte«8)  +  rawMessage  [21]  ; 
=  (temps4byte«8)  +  rawMessage  [22]; 


position . longitude  =  temps4byte; 


return  position; 

} 

/★★****★**★**★*★★*★*■*■**★*****★**★*★***★********************************* 
PROGRAM :  mi  ISecToLatLong 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Converts  a  position  expressed  totally  in  miliseconds  to 
degrees,  minutes,  seconds  and  miliseconds. 

RETURNS:  The  position  in  degrees,  minutes,  seconds  and  miliseconds. 

CALLED  BY:  navPosit  (nav.cpp) 

CALLS :  none 


latLongPosition  navigatorClass : : mi ISecToLatLong (const  latLongMilSecSc 
milSec) 

{ 

latLongPosition  position ; 
double  degrees,  minutes; 

degrees  =  (double)milSec . latitude  *  MSECS_TO_DEGREES ; 
position . latitude .degrees  =  (TWOBYTE) degrees ; 

if (degrees  <  0)  { 

degrees  =  fabs (degrees) ; 

} 

minutes  =  (degrees  -*  (TWOBYTE)  degrees )  *  60.0; 
position . latitude .minutes  =  (TWOBYTE)rainutes; 

position . latitude . seconds  =  (minutes  -  ( TWOBYTE) minutes )  *  60.0; 

degrees  =  ( double) mi ISec . longitude  *  MSECS_TO_DEGREES; 
position . longitude .degrees  =  (TWOBYTE) degrees ; 

if (degrees  <  0)  { 

degrees  =  fabs (degrees) ; 

) 

minutes  =  (degrees  -  (TWOBYTE) degrees)  *  60.0; 
position . longitude .minutes  =  (TWOBYTE) minutes ; 

position . longitude . seconds  =  (minutes  -  ( TWOBTE) minutes )  *  60.0; 
return  position; 

} 
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/*****★****★***************:*:*****★************************************** 

PROGRAM :  gr idToMi ISec 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Convert  a  grid  position  to  a  latitude  and  longitude  in  mili- 
seconds  and  returns  the  result. 

RETURNS:  The  latitude  and  longitude  in  miliseconds. 

CALLED  BY :  navPos i t  ( nav . cpp ) 

CALLS :  none 

****•*•*******★**★****★***★****★★***★******★★*★★**★★★★★****★*★*****★*★*★★/ 
void  fpeGridToMilSec (int  sig) 

{if  (sig  ==  SIGFPE)  cerr  «  "floating  point  error  in  gridToMilSec\n" ; } 

latLongMilSec  navigatorClass : igridToMilSec (const  gridSc  posit) 

{ 

signal (SIGFPE,  fpeGridToMilSec) ; 
latLongMi ISec  lat Long ; 

//  converts  grid  in  ft  to  latitude 

latLong.  latitude  =  origin .  lat  itude  +  (posit. x  /  LatToFt) 

//  converts  grid  in  ft  to  longitude 
latLong . longitude  =  origin . longitude  + 

HemisphereConversion  *  (posit. y  /  LongToFt) ; 
return  latLong; 


/★★***★*★*★****★★★*•*:★★★★*****★**★**★****★★★★**★★*************★********** 

PROGRAM:  milSecToGrid 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Convert  a  latitude  and  longitude  expressed  in  milseconds  to 
a  grid  position  in  xy  coordinates  in  feet  from  the  origin. 
RETURNS:  The  grid  position 

CALLED  BY:  navPosit  (nav. cpp),  initializeNavigator  (nav. cpp) 

CALLS :  none 

COMMENTS:  altitude  is  always  assumed  to  be  zero. 

★*★**★★*•*★★*★*★★★*★*★***★★***★★*★★*★★****★★*★****★***★*****************/ 
grid  navigatorClass : :milSecToGrid (const  latLongMi iSecS:  posit) 

{ 

grid  position; 

position.x  =  (posit . latitude  -  origin . latitude)  *  LatToFt; 
position.y  =  HemisphereConversion  * 

(posit . longitude  -  origin . longitude)  *  LongToFt; 
position. z  =  0; 

return  position; 

} 
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PROGRAM:  getGpsTime 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Parse  the  time  of  a  gps  message. 

RETURNS:  The  time  of  the  gps  message  in  seconds 

CALLED  BY:  navPosit  (nav.cpp),  initial izeNavigator  (nav.cpp) 

CALLS :  none 

★**★**★*★★****★***★★**★★**★*★*★*★*****★*★***★**************************/ 


double  navigatorClass: : getGpsTime (const  GPSdataSc  rawMessage) 

{ 

UNSIGNED_ONEBYTE  tempchar ,  hours ,  minutes ; 

UNS IGNED_FOURBYTE  t empu4by t  e ; 

double  seconds; 


hours  =  rawMessage [8] ; 
minutes  =  rawMessage [9]; 


tempchar 

tempu4byte 

tempu4byte 

tempu4byte 

tempu4byte 

seconds  = 


=  rawMessage [10] ; 

=  rawMessage [11] ; 

n  (tempu4byte<<8 )  +  rawMessage [ 12 ] ; 

=  (tempu4byte«8)  +  rawMessage  [  13  ]  ; 

=  (tempu4byte«8)  +  rawMessage[14]; 
(double) tempchar  +  ( ( (double) tempu4byte) /l.OE+9 


return  hours  *  3600.0  +  minutes  *  60.0  +  seconds; 

} 


PROGRAM:  getGpsVelocity 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Parse  the  velocity  out  of  a  gps  message. 

RETURNS:  The  velocitiy  of  the  gps  message  in  feet  per  second 

CALLED  BY:  navPosit  (nav.cpp),  init ializeNavigator  (nav.cpp) 
CALLS :  none 


double  navigatorClass :: getGpsVelocity (const  GPSdataSc  rawMessage) 

{ 

UNSIGNED_ONEBYTE  tempchar=rawMessage [31] ; 

return  (double)  (3 . 2804  *  ((tempchar  «  8)  +  rawMessage [ 32 ] )  /  100.00); 

} 

//  end  of  file  nav.cpp 
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E.  GPS.H 


#ifndef  _GPS_H 
#define  _GPS_H 


#include  <iostreain.h> 
#include  <fstream.h> 
#include  <conio.h> 

# include  "toetypes.h" 
#include  "globals .h" 
#include  "gpsPort.h” 


CLASS:  gpsClass 

AUTHOR:  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 

DATE:  11  July  1995,  last  modified  January  1997 

FUNCTION:  Reads  GPS  messages  from  the  GPS  buffer.  Checks  for  valid 
checksum  and  minimun  number  of  satellites  in  view. 


class  gpsClass  { 
public : 

//  Class  constructor  and  destructor 

gpsClass 0  {  cerr  «  " \nconstructing  gpsl"  «  endl;  } ; 

-gpsClass {)  {} 

//  returns  the  latest  gps  position  and  a  flag 
Boolean  gps  Posit  ion  (GPSdataS:)  ; 

private : 

//  calculates  the  check  sum  of  the  message 
Boolean  checkSumCheck (const  GPSdata) ; 


}; 


#endif 
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F.  GPS.CPP 


# include  <inath.h> 

# include  "gps.h" 

//  instantiates  serial  port  communications  on  comml,  global  to  allow 
//  interrupt  processing,  cleanup  to  function  properly 
gpsPortClass  portl; 

/★★★******************************^*********************************^*** 


NAME:  gpsPosition 

AUTHOR:  Eric  Bachmann,  Dave  Gay 


DATE:  11  July  1995 

FUNCTION:  Determines  if  an  updated  gps  position  message  is  available 
and  copies  it  into  the  input  argument  ' rawMessage ’ .  If  the 
message  has  a  valid  checksum  and  was  obtained  with  at  least 
three  satelites  in  view,  a  *TRUE'  is  returned  to  the  caller, 
indicating  that  the  message  is  valid. 

RETURNS:  TRUE,  if  a  valid  position  message  is  contained  in  the 

input  argument . 

CALLED  BY;  navPosit  (navigator -h) 

CALLS:  Get  (buffer.h) 

checkSumCheck  (gps.h) 


Boolean  gpsClass  :: gpsPosition  (GPSdataSc  rawMessage) 

{ 

unsigned  long  Mask{4) ; 
if  (portl .Get (rawMessage) )  { 

//  Check  for  a  valid  check  sum  and  more  the  3  satelites  and  DGPS 
return  Boolean  (  (checkSumCheck (rawMessage)  )  ScSc  (rawMessage  [39  ]  >  3) 
EcSc  (  (rawMessage  [GPSBLOCKS I ZE  -  4]  &  Mask)  ==  Mask)  )  ; 

} 

else  { 

return  FALSE;  //  No  updated  position  is  available. 

) 
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PROGRAM: 

AUTHOR: 

DATE: 

FUNCTION: 


RETURNS : 
CALLED  BY: 
CALLS: 


checkSumCheck 

Eric  Bachmann,  Dave  Gay 

11  July  1995 

Takes  an  exclusive  or  of  bytes  2  through  78  in  a  Motorola 
format  (@@EA)  position  message  and  compares  it  to  the 
checksum  of  the  message. 

TRUE,  if  the  message  contains  a  valid  checksum 

gpsPosition  (gps) 

none 


Boolean  gpsClass :: checkSumCheck (const  GPSdata  newMessage) 

{ 

BYTE  chkSum(O); 

for  (int  i  =  2;  i  <  GPSBLOCKSIZE  -  3;  i++)  { 

chkSum  newMessage  [i]  ; 

} 


return  Boolean (chkSum  ==  newMessage [GPSBLOCKSIZE  -  3]); 

) 

//  end  of  file  gps.cpp 


G.  INS.CFG 

0.1  //Konel 

0.1  //Kone2 

0.6  / /Ktwo 

0.5  //Kthreel 

0.5  //Kthree2 

0.5  //Kfourl 

0.5  //Kfour2 

1000  //tau 
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H.  INS.H 


#ifndef  _INS_H 
#define  _INS_H 

# include  <time.h> 

# include  <inath.h> 

# include  <dos . h> 

#include  <stdio.h> 

# include  <conio.h> 

#include  <fstream.h> 

#include  <iostreain.h> 

#include  ” toetypes .h" 

#include  “globals .h" 

# inc lude  " s amp 1 er . h " 

/★★★★★★★★★★•Jt************************************************************ 

CLASS:  insClass 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Takes  in  linear  accelerations,  angular  rates,  speed  and 

heading  information  and  uses  Kalman  filtering  techniques  to 
return  a  dead  reconing  position. 


class  insClass  { 
public : 

insClassO;  //  Constructor,  initializes  gains 

-insClass 0  {}  //  destructor 

Boolean  insPosit ion  ( stampedSampleSc)  ;  //  returns  ins  est .  position 

//  Updates  the  x,  y  and  z  of  the  vehicle  posture 
void  correct  Posit  ion  (stampedSampleSc,  double)  ; 

//  Sets  posture  to  the  origin  and  develops  initial  biases 
void  insSetUp  (double,  stampedSampleSc)  ; 

private : 

float  posture[6]  ;  //  ins  estimated  posture  (x  y  z  phi  theta  psi) 

double  velocities [ 6] ;  //  ins  estimated  linear  and  angular  velocities 

//  x-dot  y-dot  z-dot  phi-dot  theta-dot  psi-dot 

float  current [3];  //  ins  estimated  error  current 

//  (x-dot  y-dot  z-dot) 
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//  time  of  last  gps  position  fix 
//  filter  time  constant 


float  lastGPStime; 
int  tau; 

samplerClass  saml;  //  sampler  instance 

matrix  rotationMatrix;  I /  body  to  euler  transformation  matrix 
double  biasCorrection[3] ;  //  Software  corrections , IMU  rate  sensors 
//  Kalman  filter  gains. 

float  Konel,  Kone2,  Ktwo,  Kthreel,  Kthree2,  Kfourl,  Kfour2; 

//  Transforms  body  coords  to  earth  coords,  removes  gravity  component 
void  transformAccels  {double[]); 

//  Transforms  water  speed  reading  to  x  and  y  components 
void  transformWaterSpeed  (double,  double[]); 

//  Tranforms  body  euler  rates  to  earth  euler  rates, 
void  transformBodyRates  (double[]); 

//  Euler  integrates  the  accelerations  and  updates  the  velocities 
void  updateVelocities  (stampedSampleS:)  ; 

//  Euler  integrates  the  velocities  and  updates  the  posture 
void  updatePosture  (stampedSampleSc)  ; 

//  Builds  the  body  to  euler  rate  matrix 
matrix  buildBodyRateMatrix ( ) ; 

//  Builds  the  body  to  earth  rotation  matrix 
void  buildRotationMatrix ( ) ; 

//  Calculates  the  imu  bias  correction  during  set  up 
void  calculateBiasCorrect ions  (stampedSampleSc)  ; 

//  Applies  bias  corrections  to  a  sample 
void  applyBiasCorrect ions  (stampedSampleSc)  ; 

//  Reads  filter  constants  from  'ins.cfg' 
void  readInsConf igFile ( ) ; 


//  Post  multiply  a  matrix  times  a  vector  and  return  result, 
vector  operator*  (matrixSc,  double[]); 

#endif 
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L  INS.CPP 


#include  <iostreain.h> 
tinclude  <signal.h> 
#include  " ins .h" 
#define  SIGFPE  8 


//  Floating  point  exception 


PROGJRAM:  insClass  (constructor) 

AUTHOR:  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 

DATE:  11  July  1995 

FUNCTION:  Constructor  initializes  kalman  filter  gains  and  linear  and 

angular  velocities. 

RETURNS :  nothing 

CALLED  BY :  navigator  class 
CALLS :  none 

★  •*•★***★★★★**★★*★★****★*★★★*★*★★**★★*★*★*★★***★★★★★★*★*★★*★★★★*★★*******/ 

insClass :: insClass ( )  :  Konel(0.5),  Kone2(0.5),  Ktwo(0.6),  Kthreel { 0 . 5) , 

Kthree2  (0 .5)  ,  Kfourl(0.5),  Kf  our2  ( 0 . 5')  ,  tau{1000) 

{ 

cerr  «  " \nconstructing  insl“  «  endl; 


readInsConf igFile ( ) ; 

// 

Read  the  config  file 

velocities [0 ]  =  0.0; 

// 

X  dot 

velocities [1]  =  0.0; 

// 

y  dot 

velocities [2 ]  =  0.0; 

// 

z  dot 

velocities [3]  =  0.0; 

// 

phi  dot 

velocities [4]  =  0,0; 

// 

theta  dot 

velocities [ 5]  =  0.0; 

// 

psi  dot 

posture [ 0 ]  =0.0; 
posture [1]  =  0.0; 
posture [2]  =  0.0; 
posture [3]  =  0,0; 
posture [4]  =  0.0; 
posture [5]  =  0.0; 


//  Set  posture  to  straight  and  level  at  the  origin. 


current [0]  =  0.0; 
current [ 1 ]  =  0.0; 
current [2]  =  0.0; 


//  Initialize  error  current  to  zero 


} 
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PROGRAM;  ins Posit 

AUTHOR:  Eric  Bachinann,  Dave  Gay 


DATE:  11  July  1995 

FUNCTION:  Make  dead  reckoning  position  estimation  using  kalman 

filtering.  Inputs  are  linear  accelerations,  angular  rates, 
sp00(3^  and  heading.  Primary  input  data  is  obtained  from  a 
sampler  object  via  the  getSample  method.  This  data  is  stored 
in  the  sample  filed  of  a  stampedSample  structure  called 
newSample .  The  sample  field  is  then  used  as  a  working 
variable  as  the  linear  accelerations  and  angular  rates  it 
contains  are  converted  to  earth  coordinates  and  integrated 
to  determine  current  velocities  and  posture.  The  data  is 
complimentary  filtered  against  itself,  speed  and  magnetic 
heading . 

RETURNS:  position  in  grid  coordinates  as  estimated  by  the  INS 

CALLED  BY;  navPosit  (nav.cpp) 

CALLS:  getSample  (sampler .cpp) 

f indDeltaT  {ins .cpp) 
transf ormBodyRates  ( ins . cpp) 
buildRotationMatrix  (ins. cpp) 
transf ormAccels  (ins) 
transf ormWaterSpeed  (ins) 


void  fpelnsPosit (int  sig) 

{if  (sig  ==  SIGFPE)  cerr  «  "floating  point  error  in  insPosit\n" ; ) 


Boolean  insClass  :  :  insPosition  (stampedSampleSc  newSample) 

{ 


signal  (SIGFPE,  fpelnsPosit) ; 


double  thetaA,  phiA,  xincline,  y Incline;  //  Working  variables 

double  waterSpeedCorrection[3] ;  //  Filter  correction  for  drift 

//  and  water  speed 


if  (saml. getSample (newSample) )  { 


applyBiasCorrect ions (newSample) ; 


newSample  .  rawSample  [  0  ] 
newSample . rawSample [ 1 ] 
newSample . rawSample [ 2 ] 
newSample . rawSample [ 3 ] 
newSample . rawSample [ 4 ] 
newSample . rawSample [5] 
newSample . rawSample [ 6 ] 
newSample . rawSample [7 ] 


=  newSample . sample [ 0 ]  , 
=  newSample . sample [1]  , 
=  newSample. sample  [2] 

=  newSample  .  sample  [3  ]  , 
=  newSample . sample [4] , 
=  newSample .  sample  [5] 

=  newSample . sample [ 6 ] ; 
=  newSample.sample[7] ; 


xincline  =  newSample . sample [ 0 ]  /  GRAVITY; 
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yincline  =  (newSample . sample [ 1]  - 

(newSaitiple .  sample  [ 5 ]  *  newSample  .  sample  [  6]  )  ) 

/  (GRAVITY  *  cos (posture [4] )) ; 

if  (fabs (yincline)  >1.0)  { 

static  int  inclineCount ( 0 ) ; 
gotoxy (1,24) ; 

*  cerr  «  "Inclination  errors:  "  «  ++inclineCount  «  endl; 

.  return  FALSE ; 

} 

thetaA  =  asin (xincline)  ;  //  Calculate  low  freq  pitch  and  roll 

phiA  =  “asin(ylncline) ; 

//  Transform  body  rates  to  euler  rates. 
transformBodyRates (newSample . sample) ; 

//  Calculate  estimated  roll  rate  (phi-dot) . 

velocities [3 ]  =  newSample . sample [3 ]  +  Konel  *  (phiA  -  posture[3]); 
//  Calculate  estimated  pitch  rate  (theta-dot) . 
velocities [4]  =  newSample . sample [4 ]  +  Kone2  *  (thetaA  -  posture [ 4] ); 
//  Calculate  estimated  heading  rate  (psi-dot) . 
velocities [5]  = 

newSample .  sample  [ 5 ]  +  Ktwo  *  (newSample  . sample  [7 ]  -  posture[5]); 

//  integrate  estimated  angular  rates  to  obtain  angles 
posture[3]  +=  newSample  .del  taT  *  velocities  [3  ]; //  pitch  rate  to  angle 
posture[4]  +=  newSample  .del  taT  *  velocities  [4]  ;  //  roll  rate  to  angle 

posture[5]  +=  newSample  .del  taT  *  velocities  [5]  ;  //  yaw  rate  to  angle 

buildRotationMatrix ( ) ; 

//  Transform  accels  to  earth  coordinates 
transformAccels (newSample . sample) ; 

//  Transform  water  speed  to  earth  coordinates 

transformWaterSpeed (newSample. sample [6] ,  waterSpeedCorrection) ; 

//  Subtract  out  previous  velocity  and  apply  statistical  gain 
waterSpeedCorrection [ 0 ]  = 

Kthreel  *  (waterSpeedCorrection [ 0]  -  velocities [ 0 ]) ; 
waterSpeedCorrection [1]  = 

Kthree2  *  (waterSpeedCorrection [1]  -  velocities [1] ) ; 

//  Determine  filtered  accelerations 

newSample . sample [ 0 ]  +=  waterSpeedCorrection [ 0 ] ; 

newSample. sample [1]  +=  waterSpeedCorrection [ 1] ; 

//  Integrate  accelerations  to  obtain  velocities 
velocities [0 ]  +=  newSample . sample [ 0 ]  *  newSample . del taT; 

velocities [ 1]  +=  newSample . sample [ 1]  *  newSample . del taT; 
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velocities [2 ]  +=  newSample . sample [2 ]  *  newSample .deltaT; 

//  Integrate  velocities  to  obtain  posture 

posture [0]  +=  (velocities [ 0 ]  +  current [0])  *  newSample .deltaT; 
posture [1]  +=  (velocities [1]  +  current [1])  *  newSample .deltaT; 
posture[2]  +=  velocities [2]  *  newSample .deltaT; 


newSample .  sample  [  0  ] 
newSample . sample [ 1 ] 
newSample . sample [ 2 ] 
newSample  .  sample  [  3  ] 
newSample . sample [ 4 ] 
newSample . sample [ 5 ] 


posture [0] ; 
posture [ 1] ; 
posture [2]  ; 
posture [3 ]  ; 
posture [4] ; 
posture [5] ; 


newSample .est .x 
newSample . est .y 
newSample . est . z 


posture [0 ] ; 
posture [1] ; 
posture [2 ] ; 


return  TRUE; 

} 

else  { 

return  FALSE;  //  New  IMU  information  was  unavailable. 

} 


PROGRAM:  correctPosition 

AUTHOR:  Eric  Bachmann,  Dave  Gay 


DATE:  11  July  1995 

FUNCTION:  Reinitializes  the  INS  based  on  a  known  position  and  computes 
apparent  current  based  on  past  accumulated  errors  of  the  INS. 
It  is  called  by  the  navigator  each  time  a  new  GPS  (true)  fix 
is  obtained. 

RETURNS:  void 

CALLED  BY:  navPosit  (nav) 

CALLS :  none 


void 

insClass : :correctPosition (stampedSampleSc  posit,  double  positTime) 

{ 

double  deltaT; 

if  (positTime  <  lastGPStime)  {  //  Correct  for  new  day  if  necessary 

positTime  +=  86400; 

} 

deltaT  =  positTime  ~  lastGPStime;  //  Find  time  since  last  gps  fix. 
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//  Determine  INS  error  since  last  gps  fix 
double  deltaX  =  posit. est.x  -  posture[0]; 
double  deltaY  =  posit. est.y  -  posturetl]; 

//  Reinitialize  posture  to  known  position  (gps  fix) 
posture[0]  =  posit. est.x; 
posture [1]  =  posit. est.y ; 

posture [2]  =0.0;  //  Unit  is  assumed  to  be  on  the  surface 

//  Add  gain  filtered  error  to  previous  errors 

posit . current [ 0 ]  =  current[0]  +=  Kfourl  *  (deltaX  /  deltaT); 

posit . current [ 1]  =  current[l]  +=  Kfour2  *  (deltaY  /  deltaT); 

//  Save  the  time  of  the  gps  fix  for  next  calculation 
lastGPStime  =  positTime; 


PROGRAM:  insSetUp 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Initializes  the  INS  system.  Sets  the  posture  to  the  origin. 

Initializes  the  heading  using  magnetic  compass  information. 

Initializes  the  last  GPS  fix  and  last  IMU  information  times. 
RETURNS :  void 

CALLED  BY:  initial izeNavigator  (nav) 

CALLS :  calculateBiasCorrect ions  ( ins ) 

getSample  (sampler) 
bui IdRotat ionMatr ix  ( ins ) 
transf ormWaterSpeed  ( ins ) 

***********************************************************************/ 


void  fpelnsSetUp ( int  sig) 

{if  (sig  ==  SIGFPE)  cerr  «  "floating  point  error  in  inSetUpXn";} 
void  insClass: :insSetUp (double  originTime,  stampedSampleSc  posit) 


cerr  «  "  Initializing  INS."  «  endl; 
signal  (SIGFPE,  fpelnsSetUp); 

saml.initSampler 0 ;  //  Initialize  the  sampler 

calculateBiasCorrections (posit ) ;  //  set  imu  biases 


posture[5]  =  posit . sample [7 ] ;  //set  initial  true  heading 

buildRotationMatrixO  ;  //set  initial  speed 

trans f ormWaterSpeed (posit . sample [ 6 ] ,  velocities) ; 


109 


posit.current [0]  =  0.0; 
posit .current [1]  =  0.0; 
posit .current [2]  =  0.0; 


lastGPStime  =  originTime; 


//  initialize  times 


cerr  « 

} 


INS  initialization  complete.”  «  endl; 


PROGRAM :  t ran s  f ormAc  cels 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Transforms  linear  accelerations  from  body  coordinates  to 

earth  coordinates  and  removes  the  gravity  component  in  the 
z  direction. 

RETURNS:  void 

CALLED  BY :  navPos i t 
CALLS :  none 


void  insClass : : transformAccels  (double  newSample [ ] ) 

{ 

vector  earthAccels; 

newSample[0]  -=  GRAVITY  *  sin (posture [4] ) ; 

newSample [1]  +=  GRAVITY  *  sin (posture [3 ] )  *  cos (posture [4] ) ; 
newSample [2]  +=  GRAVITY  *  cos (posture [ 3 ] )  *  cos (posture [ 4 ]) ; 

earthAccels  =  rotationMatrix  *  newSample; 

newSample[0]  =  earthAccels . element [ 0 ] ; 
newSample[l]  =  earthAccels . element [ 1] ; 
newSample[2]  =  earthAccels .element [2] ; 

} 


**★*★*★**★★★***★***★***★* ************** 


PROGRAM :  trans  f ormWat erSpeed 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Transforms  water  speed  into  a  vector  in  earth  coordinates  and 
returns  them  in  the  speedCorrection  variable. 


*  ★ 


RETURNS:  void 

CALLED  BY:  navPosit 
CALLS :  none 


/ 


no 


void  insClass : : transformWaterSpeed  (double  waterSpeed,  double 
speedCorrect ion [ ] ) 

{ 

double  water [3]  =  {waterSpeed,  0.0,  0.0}; 
vector  waterVelocities  =  rotationMatrix  *  water; 

speedCorrect ion  [0]  =  waterVelocities.element[0]; 

spee’dCorr action  [1]  =  waterVelocities  .  element  [1]  ; 

speedCorrection  [2]  =  waterVelocities . element [2 ] ; 

} 


PROGRAM: 
AUTHOR : 
DATE: 
FUNCTION: 
RETURNS : 
CALLED  BY: 
CALLS: 


transformBody Rates 
Eric  Bachmann,  Dave  Gay 
11  July  1995 

Tranforms  body  euler  rates  to  earth  euler  rates 

none 

insPosit 

buildBodyRateMatrix 


void  insClass :: transformBodyRates  (double  newSample[]) 

{ 

vector  earthRates  =  buildBodyRateMatrix ( )  *  & (newSample [3 ] ) ; 

newSample[3]  =  earthRates . element [ 0 ] ; 
newSample[4]  =  earthRates . element [ 1] ; 
newSample[5]  =  earthRates . element [2 ] ; 

} 


/ 


PROGRAM: 

AUTHOR: 

DATE: 

FUNCTION: 

RETURNS : 

CALLED  BY: 

CALLS: 


bu i 1 dBody Ra t  eMa t  r i x 
Eric  Bachmann,  Dave  Gay 
11  July  1995 

Builds  body  to  Euler  rate  translation  matrix. 

rate  translation  matrix 

insPosit 

none 


matrix  insClass: :buildBodyRateMatrix ( ) 

{ 

matrix  rateTrans; 

float  tth  =  tan(posture[4] ) , 
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sphi  =  sin (posture [ 3] ) , 
cphi  =  cos (posture [3 ]) , 
cth  =  cos (posture [4T); 


rateTrans . element [ 0 ] [ 0 ] 
rateTrans . element [ 0 ] [ 1 ] 
rateTrans . element [ 0 ] [2] 
rateTrans . element [ 1 ] [ 0 ] 
rateTrans . element [ 1 ] [ 1 ] 
rateTrans .element [1] [2] 
rateTrans .element [2] [0] 
rateTrans .element [2] [1] 
rateTrans . element [2] [2] 


1.0; 

tth  *  sphi 7 
tth  *  cphi; 
0.0; 
cphi  ; 

-sphi ; 

0.0; 

sphi  /  cth; 
cphi  /  cth; 


return  rateTrans; 

} 


PROGRAM:  buildRotationMatrix 
AUTHOR:  Eric  Bachmann,  Dave  Gay 
DATE:  11  July  1995  . 

FUNCTION:  Sets  the  body  to  earth  coordinate  rotation  matrix. 

RETURNS :  void 

CALLED  BY;  insPosit,  insSetUp 

CALLS :  none 


void  insClass: :buildRotationMatrix ( ) 

{ 

float  spsi  =  sin (posture [5] ) , 
cpsi  =  cos (posture [ 5 ]) , 
sth  =  sin (posture [4] ) , 
sphi  =  sin (posture [3 ]) , 
cphi  =  cos (posture [3 ]) , 
cth  =  cos (posture [4]); 


rotationMatrix .element [ 0 ] 

[0] 

= 

cpsi  ■ 

*  cth; 

rotationMatrix . element [ 0 ] 

[1] 

= 

(cpsi 

*  sth  * 

sphi)  - 

(spsi 

*  cphi) 

rotationMatrix .element [ 0 ] 

[2] 

= 

(cpsi 

*  sth  * 

cphi )  + 

(spsi 

*  sphi) 

rotationMatrix. element [1] 

[0] 

= 

spsi  ' 

cth; 

rotationMatrix. element [1] 

[1] 

(cpsi 

*  cphi ) 

+  (spsi 

*  sth 

*  sphi) 

rotationMatrix. element [ 1] 

[2] 

= 

(spsi 

*  sth  * 

cphi )  - 

(cpsi 

*  sphi ) 

rotationMatrix. element [2 ] 

[0] 

= 

-sth; 

rotationMatrix .element [ 2 ] 

[1] 

= 

cth  * 

sphi  ; 

rotationMatrix . element [2] 

[2] 

= 

cth  * 

cphi ; 
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PROGRAM: 

AUTHOR: 

DATE: 

FUNCTION: 

RETURNS : 
CALLED  BY: 


postmultiplication  operator  * 

Eric  Bachmann,  Dave  Gay 
11  July  1995 

Post  multiply  a  3  X  3  matrix  times  a  3  X  1  vector  and 
return  the  result. 

3X1  vector 


CALLS :  None 


vector  operator*  (matrixS:  transform,  double  state []) 

{ 

vector  result ; 

for  (int  i  =  0;  i  <  3;  i++)  { 

result . element [i]  =  0.0; 

for  (int  j  =  0;  j  <  3;  j++)  { 

result . element [ i ]  +=  transform. element [ i] [j ]  *  state[j]; 

) 

) 

return  result; 


^***^******************************************************************* 
PROGRAM:  calculateBiasCorrect ions 

AUTHOR:  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 


DATE:  11  July  1995 

FUNCTION:  Calculates  the  initial  imu  bias  by  averaging  a  number  of 


imu  readings. 
RETURNS :  none 

CALLED  BY:  insSetup 
CALLS :  none 


void  fpeCalculateBiasCorrections (int  sig) 

{if  (sig  ==  SIGFPE)  cerr  «  "floating  point  error  in 
CalculateBiasCorrectionsXn" ; } 


void  insClass: :calculateBiasCorrections (stampedSampleS:  biasSample) 

{ 

signal  (SIGFPE,  fpeCalculateBiasCorrections) ; 


int  biasNuinber  { tan/10)  ; 


biasCorrection [ 0 ]  =  0.0; 
biasCorrect ion [ 1 ]  =  0.0; 
biasCorrection [ 2 ]  =  0.0; 


//  p  roll  rate 
//  q  pitch  rate 
//  r  yaw  rate 


for  (int  i  =  0;  i  <  biasNumber;  i++)  { 

U  Find  the  average  of  the  biasNumber  packets 
while ( I saml .getSample (biasSample) )  {/*  */}; 

biasCorrection [0]  +=  biasSample . sample [3 ] /biasNumber ;  //  roll-rate/ 
b# 

biasCorrection [1]  +=  biasSample . sample [4] /biasNumber ;  //pitch-rate/ 
b# 

biasCorrection [2]  +=  biasSample .sample [5] /biasNumber;  //  yaw-rate /b# 

) 

//  set  biasSample  correction  fields  to  new  bias  correction  values 
//  negative  biasCorrection  value  is  taken  so  biases  are  added  to  sensor 
values 

biasSample. bias [3]  =  biasCorrection [0  ]  =  -(biasCorrection[0]); 
biasSample. bias [4]  =  biasCorrection [1]  =  -(biasCorrection[l]); 
biasSample. bias [5]  =  biasCorrection [2]  =  - (biasCorrection [2]); 


/★★*★**★★*★★**★**★★★★★★**★★★*★*★*★***★★***★★★**★*★★**★****★************* 
PROGRAM:  applyBiasCorrections 

AUTHOR:  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 

DATE:  11  July  1995 

FUNCTION:  Applies  updated  bias  corrections  to  a  sample. 

RETURNS:  void 

CALLED  BY:  insPosit 
CALLS :  none 

***********************************************‘^***********************/ 


void  insClass :: applyBiasCorrections {stampedSainple&  posit) 

{ 

const  float  sampleWght (posit .deltaT/tau) ; 
const  float  biasWght(l  -  sampleWght); 

//Calculate  updated  bias  values 

biasCorrection [0]  =  (biasWght  *  biasCorrection [ 0] ) 

-  (sampleWght  *  posit . sample [3 ]) ; 
biasCorrection [1]  =  (biasWght  *  biasCorrection [ 1] ) 

-  (sampleWght  *  posit . sample [4 ]) ; 
biasCorrection [2]  =  (biasWght  *  biasCorrection [2 ] ) 

-  (sampleWght  *  posit . sample [5 ]) ; 
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posit  .sample  [3]  +=- biasCorrection  [0]  ;  //Apply  the  bias  to  the  sample 
posit . sample [4]  +=  biasCorrection[l]; 
posit . sample [5]  +=  biasCorrection[2]; 

posit .bias [ 3 ]  =  biasCorrection [ 0 ] ;  //Save  the  bias  to  the  sample 

posit .bias [4]  =  biasCorrection [ 1] ; 
posit .bias [5]  =  biasCorrection [2]; 


PROGRAM: 

AUTHOR: 

DATE: 

FUNCTION: 

RETURNS : 

CALLED  BY: 

CALLS: 


readInsConf igFile 

Rick  Roberts,  Eric  Bachmann 

02  Nov  96 

Reads  filter  constants  from  ‘ins.cfg* 
void 

ins  class  constructor 
none 


void  insClass :: readInsConf igFile ( ) 

{ 

cerr  «  "Reading  ins  configuration  file."  «  endl; 
if stream  insCfgFile ( " ins . cfg" ,  ios::in); 
if ( linsCfgFile)  { 

cerr  «  "could  not  open  ins  configuration  file!"  «  endl; 

} 

else  { 

char  comment [128] ; 
insCfgFile 

»  Konel  »  comment 
»  Kone2  »  comment 
»  Ktwo  »  comment 
»  Kthreel  »  comment 
»  Kthree2  »  comment 
»  Kfourl  »  comment 
»  Kfour2  »  comment 
»  tau  »  comment; 

} 

insCfgFile .close { ) ; 

} 

//  end  of  file  ins.cpp 
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J.  SAM.CFG 


1.0 

pScale (roll ) 

1.0 

qScale (pitch) 

1.0 

rScale (yaw) 

1.34 

xAccelScale (pitch) 

1.34 

yAccelScale (roll ) 

1.34 

zAccelScale (yaw) 

1.827 

waterSpeedScale 

K.  SAMPLER.H 

#ifndef  _SAMPLER_H 
#define  _SAMPLER_H 


# include 
# include 
# include 
#include 
#include 
#include 
#include 


<tiine  .h> 
<math . h> 

<dos . h> 
<conio .h> 
<stdio.h> 

<f stream. h> 
<iostream,h> 


# include 
tinclude 
#include 
#include 


'*  toetypes  .h" 
“globals .h" 
"a2d.h" 
"compass .h" 


#define  MAX_SAMPLE_NUM  1000 

#define  xyAccelLimit  0NE_G  //  Max  accell  in  x  and  y  direction 

^define  zAccelLiinit  2  *  0NE_G  / /  Max  accel  in  z  direction 

idefine  rateLimit  0.872665  //  Max  rotational  rate  in  radians 

#define  speedLimit  25.3  //  Max  water  speed 

#define  headingLimit  2  *  M_PI 


const  int  INBUFFSIZE  =  512; 


CLASS :  samplerClass 

AUTHOR:  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 

DATE:  11  July  1995,  last  modified  January  1997 

FUNCTION:  Formats,  timestamps,  low  pass  filters  and  limit  checks  IMU, 
water-speed  and  heading  information. 

COMMENTS:  This  class  is  extremely  dependent  upon  the  specific 

hardware  configuration.  It  is  designed  to  isolate  the 
INS  from  these  particulars. 
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class  sainplerClass  -{ 


public : 


samplerClass { ) ;  //  Class  constructor,  destructor 

-samplerClass ( )  {} 

‘Boolean  initSampler ( ) ;  //  Initializes  Sampler 

//  checks  for  the  arrival  of  a  new  sample  and  formats  it 
Boolean  get  Sample  (stampedSampleS:); 


private : 

float 

pScale; 

// 

roll 

float 

qScale; 

// 

pitch 

float 

r Scale; 

// 

yaw 

float 

xAccelScale; 

// 

pitch 

float 

yAccelScale ; 

// 

roll 

float 

zAccelScale; 

// 

yaw 

float 

waterSpeedScale ; 

compassClass  compl;  //  instantiate  member  compass  object 

a2dClass  a2dl;  //  instantiate  member  a2d  object 

//  stores  incoming  FIFO  samples  by  channel 
float  sample  [MAX_SAMPLE_NUM]  [8]  ; 


int 

subSample Index ; 

// 

int 

sampleIndex; 

// 

int 

sampleCount ; 

// 

float  samplePeriod; 

Boolean  readSamples  (stampedSampleSc 

counts  channels 
indexes  samples ' 
counts  samples 

newSample)  ; 


array 


void  f  ilterSample  ( stampedSampleSc  newSample); 

void  format  Sample  (stampedSampleSc  newSample); 

void  increment  (intSc  index) 

{  if  (++index  ==  MAX_SAMPLE_NUM)  index  =0;} 

void  decrement  ( intS:  index) 

{  if  (—index  <  0)  index  =  MAX_SAMPLE_NUM  -  1;} 
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//  Reads  filter  constants  from  ‘sam.cfg* 
void  readSamplerConf igFile ( ) ; 

double  pUnits (double  angular) 

{  return 

(pScale  *  ( ( (angular-2047 . 0 )  /  2047.0  )  *  50.0)  *  (M_PI/180 . 0 ) ) ; } 

double  qUnits (double  angular) 

{  return 

(qScale  *  (( (angular-2047 . 0 )  /  2047.0  )  *  50.0)  *  (M_PI/180 . 0 )  )  ; } 

double  rUnits (double  angular) 

{  return 

(rScale  *  ((  (angular-2047 . 0 )  /  2047.0  )  "^50.0)  *  (M_PI/180 . 0 )  )  ;  } 
double  xAccelUnits (double  linear) 

{  return  (xAccelScale  *  ( (linear-2047 . 0 )  /  2047.0  )  *  GRAVITY);} 
double  yAccelUnits (double  linear) 

{  return  (yAccelScale  *  {( linear-2047 . 0 )  /  2047.0  )  *  GRAVITY);} 

double  zAccelUnits (double  linear) 

{  return 

(zAccelScale  *  (( linear-2047 . 0 )  /  2047.0)  *  (2.0  *  GRAVITY));} 
double  depthUnits (double  depth) 

{  return  (((depth  -  819.0)  /  (4095.0-819.0))  *  180.0);} 

double  waterSpeedUnits (double  speed)  //feet  per  second 
{  return  (waterSpeedScale  *  ((speed  -  2047.0)  /  2048.0)  *  25.3);} 

}; 

#endif 
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L.  SAMPLER.CPP 


#include  “sampler. h" 

y*********************************************************************** 

PROGRAM:  samplerClass  Constructor 

AUTHOR:  Eric  Bachmann,  Randy  Walker,  Rick  Roberts 

DATE.:  12  May  1995,  last  modified  December  1996 

FUNCTION:  Constructs  saml,  initializes  default  config  values,  calls 
readSamplerConf igFile  to  read  any  updated  values. 

RETURNS :  saml 

CALLED  BY:  insSetUp  (ins.cpp) 

CALLS:  readSamplerConf igFile 


samplerClass : : samplerClass ( ) 

:  samplelndex{0) ,  subSamplelndex(O) , 

samplePeriod (a2dl . chcnt  *  a2dl.delta_t  *  0.000001), 
pScale(O.O),  qScale(O.O),  rScale(O.O), 

xAccelScale(O.O) ,  yAccelScale (0 . 0 ) ,  zAccelScale{0 .0) , 
waterSpeedScale (0.0) 


{ 

cerr  «  " \nconstructing  sampler  w/  a2dl,  compl"  «  endl; 
readSamplerConf igFile ( )  ; 

) 


PROGRAM:  initSampler 

AUTHOR:  Eric  Bachmann,  Randy  Walker,  Rick  Roberts 

DATE:  12  May  1995 

FUNCTION:  Instantiates  the  compass  A2D  objects. 

RETURNS :  TRUE 

CALLED  BY:  insSetUp  (ins.cpp) 

CALLS:  initCompass ( ) ,  A2D  member  functions 

★  ★**★*★★*•*****•*■*★* -A- *********★**★★★***★**★★*★*★★*★**★★★*★*★**★**★*★*★★**/ 


Boolean  samplerClass : : initSampler ( ) 

{ 

cerr  «  "  Initializing  Sampler"  «  endl; 

compl . initCompass ( ) ; 

cerr  «  "  Initializing  A2D."  «  endl; 

a2dl.initA2d( ) ; 
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cerr  « 


■A2D  initialization  complete.”  «  endl; 


cerr  «  "  Sampler  initialization  complete."  «  endl; 

return  TRUE; 


PROGRAM:  get  Sample 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Prepares  raw  sample  data  for  use  by  the  INS  object 
RETURNS:  TRUE,  if  a  valid  sample  was  obtained 

CALLED  BY:  ins Posit  (ins) 
insSetup  (ins) 

CALLS:  readSamples  (sampler) 

filterSample  (sampler) 
formatSample  (sampler) 


Boolean  samplerClass  :  :getSample  (stampedSampleSc  newSample) 

{ 

if  (readSamples  (newSample)  )  {  //  checks  for  the  arrival  of  a  new  sample 

filterSample (newSample) ; 
formatSample (newSample) ; 
return  TRUE; 

) 


return  FALSE; 

) 


//  Sample  packet  not  available 


/ 


PROGRAM: 
AUTHOR : 
DATE: 
FUNCTION: 


RETURNS : 


readSamples 

Eric  Bachmann,  Randy  Walker 
12  May  1996 

Retrieves  all  samples  of  the  IMU,  water  speed,  and  depth 
that  are  present  in  the  A2D  FIFO  until  the  FIFO  is  EMPTY, 
Calculates  delta_t . 

TRUE  -  There  were  new  samples  pulled  from  the  FIFO 
FALSE  -  There  were  no  new  samples 


CALLED  BY:  getSample 

CALLS :  getFifoStatus ( ) ,  getFifoData ( ) 
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Boolean  saraplerClass':  : readSamples  ( stampedSamplefi:  newSample) 

{ 

static  int  overf lowCount ( 0 ) ; 

if  (a2dl.getFifoStatus()  ==  FULL)  {  //  Did  the  FIFO  overflow? 

gotoxy (1 , 19 ) ; 

cout  «  "FIFO  Overflowed,  #:  "  «  ++overf lowCount 

«  "  reiniting  a2d"  «  endl; 
a2dl . reinitA2d ( ) ; 
return  FALSE; 


if  (a2dl.getFifoStatus{)  !=  EMPTY)  {  //  Does  the  FIFO  have  new  samples? 

sampleCount  =  0;  //  Counts  the  number  of  samples  taken 

while  (a2dl.getFifoStatus()  i=  EMPTY)  {  //  Empty  the  FIFO 

sample [sampleindex] [subSampleIndex++ ]  =  a2dl . getFifoData ( ) ; 

//  Has  it  pulled  one  sample  of  each  channel  from  the  FIFO? 
if  (subSample Index  ==  8)  { 

subSampleIndex=  0; 

increment (sampleindex) ;  //  set  to  record  next  sample 

++ sampleCount ; 

} 

} 

if  (sampleCount  >0)  { 

//  calculate  time  delta 

newSample .del taT  =  sampleCount  *  samplePeriod; 
return  TRUE; 

} 

else  {  //  No  full  samples 

return  FALSE; 

} 

) 

else  {  //No  new  samples 

return  FALSE; 

} 
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/*★*★****★***★*★*★*★*★★★★.*★****★********★**★*★****★*****************★*** 

PROGRAM:  f  ilterSainple 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Low  pass  filters  eight  closely  spaced  sets  of  sensor 

readings  by  summing  the  readings  of  each  sensor  and  computing 
the  average . 

RETURNS:  void 

CALLED  BY:  getSample 

CALLS :  none 

void  samplerClass  :  :  f  ilterSample  (stampedSampleS:  newSample) 

{ 

for  (int  i  =  0;  i  <  8;  i++)  { 
newSample .sample [i]  =  0; 

} 

int  j (sample Index) ; 

for  (i  =  0;  i  <  sampleCount;  i++)  { 

decrement (j); 

newSample .sample [0]  +=  sample[j][0]  /  sampleCount; 
newSample . sample [1]  +=  sample[j][l]  /  sampleCount; 
newSample . sample [2]  +=  sample[j][2]  /  sampleCount; 
newSample . sample [ 3 ]  +=  sample[j][3]  I  sampleCount; 
newSample . sample [4]  +=  sample[j][4]  /  sampleCount; 
newSample . sample [5]  +=  sample[j][5]  /  sampleCount; 
newSample. sample [6]  +=  sample[j][6]  /  sampleCount; 
newSample. sample [7]  +=  sample[j][7]  /  sampleCount; 

} 

} 


/*-*-*******************************************************************:*r* 

PROGRAM :  f o  rma t  Samp 1 e 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Converts  integers  representing  voltage  readings  into 
real  world  units  which  are  useable  by  the  INS . 

RETURNS:  void 

CALLED  BY: getSample 
CALLS :  none 
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void  samplerClass  :  :  formatSample  ( stampedSampleSc  newSample) 


newSample . sample [ 0 ] 
newSample . sample [ 1 ] 
newSample . sample [ 2 ] 


xAccelUnits (newSample. sample [0] )  ; 
yAccelUnits (newSample. sample [1] ) ; 
zAccelUnits (newSample. sample [2] ) ; 


newSample . sample [ 3 ] 
newSample . sample [4] 
newSample . sample [ 5 ] 


pUnits (newSample. sample [3] ) ; 
qUnits (newSample . sample [4] ) ; 
rUnits (newSample . sample [5] ) ; 


newSample . sample [ 6 ] 
newSample . sample [ 7 ] 


waterSpeedUnits (newSample . sample [ 6] ) ; 
compl . getHeading ( ) ; 


PROGRAM: 
AUTHOR : 
DATE: 
FUNCTION: 
RETURNS ; 
CALLED  BY: 
CALLS: 
COMMENTS : 


reads amp 1 er Con f i gF i 1 e 
Rick  Roberts,  Eric  Bachmann 
02  Nov  96 

Reads  filter  constants  from  'ins.cfg* 
void 

ins  class  constructor 
none 

*  Do  not  allow  blanks  in  'comment*  section  of  sam.cfg  * 


void  samplerClass: : readSamplerConf igFile ( ) 

{ 

FILE  ^samCfgFile; 

if  ((samCfgFile  =  fopen ( "sam.cfg" ,  "r"))  ==  NULL) { 

cerr  «  "could  not  open  sampler  configuration  filel"  «  endl; 

} 

else  { 

cerr  <<  "\nReading  Sampler  configuration  file."  «  endl; 
char  line[128]; 

fscanf (samCfgFile, "%f%s" , Upscale, line) ; 
cerr  «  "pScale:  "  «  pScale  «  endl; 

fscanf  (samCfgFile,  "%f%s" ,  ScqScale,  line)  ; 
cerr  <<  "qScale:  "  «  qScale  «  endl; 

fscanf (samCfgFile, "%f %s " , &rScale, line) ; 
cerr  «  "rScale:  "  «  rScale  «  endl; 

fscanf  ( samCfgFile,  " %f %s " ,  ScxAccelScale ,  line)  ; 
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cerr  «  "xAccelScale :  "  «  xAccelScale  «  endl; 

f  scanf  (samCfgFile,  "%f%s” ,  S:yAccelScale,  line)  ; 
cerr  «  “yAccelScale :  "  «  yAccelScale  «  endl; 

f  scanf  (samCfgFile,  “%f%s“ ,  SczAccelScale,  line)  ; 
cerr  «  “ zAccelScale :  “  «  zAccelScale  «  endl; 

f scanf (samCfgFile, " %f %s " , &waterSpeedScale, line) ; 

cerr  «  “waterSpeedScale :  ”  «  waterSpeedScale  «  endl; 


} 

fclose (samCfgFile) ; 

} 

//  end  of  file  sampler. cpp 


M.  COMPASS,H 


#ifndef  _COMPASS_H 
#define  _COMPASS_H 


tinclude  <iostream.h> 
#include  <fstream.h> 

# include  <conio.h> 


# include  ’* toetypes  .  h" 
#include  “globals .h" 

# include  "compport  .h*‘ 


BYTE  asciiToHex(BYTE) ;  //  conversion  function  prototype 

/***********★★****★★*******★**★★***★*★★********************************* 
CLASS :  compassClass 

AUTHOR:  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 

DATE:  11  July  1995,  last  modified  January  1997 

FUNCTION:  Reads  compass  messages  from  the  compass  buffer.  Checks  for 

valid  checksum.  Corrects  heading  for  magnetic  variation. 
Heading  is  continuous.  There  is  no  branch  cut  at  360  degrees. 


class  compassClass  { 
public : 

//  class  constructor  and  destructor 

compassClass ( )  :  currentHeading (0.0) 

{  cerr  «  “Compass  constructed."  «  endl;  } 
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-compassClass ( )  {} 


float  initCompass ( ) ;  //  initialize  currentHeading 

float  getHeading ( ) ;  //  returns  the  latest  heading 

private : 

//  Maintains  the  most  recently  obtained  heading, 
float  currentHeading; 

//  calculates  the  check  sum  of  the  message 
Boolean  checkSumCheck (const  compData) ; 

//  Parses  a  selected  field  out  of  a  compass  message, 
float  par seCompData (const  compData,  const  BYTE) ; 

//  Converts  magnetic  direction  based  on  magnetic  variation, 
float  trueHeading (const  float); 

//  Returns  the  heading  without  branch  cuts 
float  continousHeading (const  float); 

}; 

#endif 

N,  COMPASS.CPP 

# include  <math.h> 

#include  <stdlib.h> 

# include  “compass .h" 

//  instantiates  serial  port  communications  on  comm2,  global  to  allow 
//  interrupt  processing,  cleanup  to  function  correctly 
compassPortClass  port2 ; 

/★*★★*★★*★****★****★*★★*★★★★****★★★★★*******★★★***★********************* 


NAME: 

AUTHOR: 

DATE: 

FUNCTION: 


initCompass 

Eric  Bachmann,  Dave  Gay,  Rick  Roberts 
11  July  1995 

Determines  if  a  valid  compass  message  is  held  in  the 
compass  buffer  and  initializes  currentHeading  to  that  value. 
Will  attempt  10  times  with  a  built  in  delay  and  then  exit 
with  a  warning  if  a  valid  heading  is  not  obtained. 


RETURNS :  currentHeading 

CALLED  BY:  INSsetUp  (ins.cpp) 

CALLS:  Get  (buffer. h)  parseCompData  (compass .cpp) 

checkSumCheck  (gps .h)  continuousHeading  (compass . cpp) 

trueHeading  (compass .cpp) 
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float  compassClass : : initCompass { ) 

{ 

cerr  «  "  Initializing  Coinpass"  «  endl; 

Boolean  compF lag (FALSE) ; 
float  tempHeading; 
compData  rawMessage; 

//  try  10  times  to  get  a  valid  message 

for  (int  i  =  1  ;  { (i  <  10)  &&  (compFlag  ==  FALSE) ) ;  i++  )  { 

if  ( (port 2 .headings .Get (rawMessage) )  &&  (checkSumCheck (rawMessage) ) ) { 
tempHeading  =  parseCompData (rawMessage,  *C')  *  degToRad; 
currentHeading  =  continousHeading (trueHeading (tempHeading) ) ; 
compFlag  =  TRUE; 

} 

else  {  //  invalid  message  -  delay 

delay (1000) ; 

} 

} 

if  (compFlag  ==  FALSE)  { 

cerr  «  "\nWARNING:  UNABLE  TO  OBTAIN  INITIAL  COMPASS  HEADING  1“ 

«  endl ; 
delay (2000) ; 

} 

else  { 

cerr  «  "  Compass  initialization  complete."  «  endl; 

} 

return  currentHeading; 


NAME: 
AUTHOR : 
DATE: 
FUNCTION: 


RETURNS : 
CALLED  BY: 
CALLS : 


getHeading 

Eric  Bachmann,  Dave  Gay,  Rick  Roberts 
11  July  1995 

Determines  if  an  updated  compass  message  is  available  and 
copies  it  into  the  input  argument  ‘rawMessage'.  If  the 
message  has  a  valid  checksum,  currentHeading  is  returned 
to  the  caller,  currentHeading  is  also  the  default  return, 
currentHeading 
navPos i t  ( navigator . h ) 

Get  (buffer.h) 
checkSumCheck  (compass .cpp) 


float  compassClass : : getHeading ( ) 

{ 
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float  tempHeading; 

Boolean  checkSumFlag; 
c  ompDa  t  a  r awMe  s  s  ag  e ; 

if  ( (port2 .headings. Get (rawMessage) )  &&  (checkSumCheck (rawMessage) ) )  { 

tempHeading  =  parse CompDat a (rawMessage,  *C')  *  degToRad; 
current Heading  =  continousHeading (trueHeading (tempHeading) ) ; 

return  currentHeading; 

} 

else  { 

return  currentHeading;  //  No  updated  position  is  available. 

) 

} 


*★**★**★**★*★*****★**★★**★★★**★★★★*★*★★***★★★★***★***★* ************* 

NAME:  asciiToHex 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Administrative  conversion  function 

RETURNS:  Hex  version  of  an  ascii  character 

CALLED  BY :  checkSumCheck 

CALLS :  None 

BYTE  asciiToHex (BYTE  letter) 

{ 

if  (letter  >=  ' A* )  { 

return  (letter  ~  'A'  +  10); 

} 

else  { 

return  (letter  -  48); 

} 

} 


/★*★**★**★★****★★★***★*****★*★*★★★*★*★*★*****★★★*★★**★*****★****★****★*★ 

PROGRAM :  checkSumCheck 

AUTHOR:  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Calculates  the  checksum  of  the  compass  message  and 

compares  it  to  the  indicated  checksum  of  the  message. 
RETURNS:  TRUE,  if  the  message  contains  a  valid  checksum 

CALLED  BY:  initCompass,  getHeading 
CALLS :  none 
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Boolean  compassC  las  s';  rcheckSumCheck  (const  compData  newMessage) 

{ 

BYTE  calChkSum(O) ; 

BYTE  mesChkSuin(O)  ; 

for  (int  1=1;  newMessage [ i]  i=  i++)  { 

calChkSum  newMessage [ i] ; 

)  . 

mesChkSum  =  asciiToHex (newMessage [ i+1] )  *  16 

+  asciiToHex (newMessage [i+2] ) ; 

return  Boolean (cal ChkSum  ==  mesChkSum) ; 

} 


/ 


PROGRAM; 

AUTHOR: 

DATE: 

FUNCTION: 

RETURNS : 
CALLED  BY: 

CALLS : 


trueHeading 

Eric  Bachmann,  Dave  Gay 
11  July  1995 

Converts  magnetic  direction  to  true  based  on  local 

magnetic  variation. 

true  heading 

insPosit 

insSetUp 

none 


float  compassClass: : trueHeading (const  float  magHeading) 

{ 

static  double  twoPi(2.0  *  M_PI) ; 

double  trueHeading  =  magHeading  +  RADIANMAGVAR; 

if  (trueHeading  >  twoPi)  { 

trueHeading  -=  twoPi; 

) 

return  trueHeading; 

} 


128 


PROGRAM :  cont inousHeading 

AUTHOR :  Eric  Bachmann 


DATE: 
FUNCTION: 
RETURNS : 
CALL-ED  BY: 


11  July  1995 

Maintains  track  of  branch  cuts  &  returns  a  continous  heading, 
continous  true  heading 
insPosit ,  insSetUp 


CALLS :  none 


float  compassClass: : cont inousHeading (const  float  trueHeading) 

{ 

const  float  twoPi(2.0  *  M_PI) ; 

static  int  branchCutCount ( 0 ) ; 

static  float  previousHeading (trueHeading) ; 

if  ((4.71  <  previousHeading)  ScSc  (trueHeading  <  1.57)  ){ 

++branchCutCount ;  //  Went  through  North  in  a  right  hand  turn 

) 

else  { 

if  ((1.57  >  previousHeading)  &&  (trueHeading  >  4.71))  { 

— branchCutCount;  //  Went  through  North  in  a  left  hand  turn 

} 

} 

previousHeading  =  trueHeading; 

return  trueHeading  +  (branchCutCount  *  twoPi) ; 

} 

^*********************************************************************** 


PROGRAM: 

AUTHOR: 

DATE: 

FUNCTION: 

RETURNS : 

CALLED  BY: 

CALLS: 


parseCompData 
Eric  Bachmann 
11  July  1995 

Parses  the  heading  out  of  a  compass  message, 
the  message  heading  as  a  float 
insPosit,  insSetUp 
none 


float  compassClass :: parseCompData (const  compData  rawMessage, 

const  BYTE  key) 


{ 

f loat  dataSum ( 0 ) ; 


for (int  j  =  0;  rawMessage [ j ]  i=  key;  j++){} 


j++;  *  ■ 

for{int  i  =  0;  rawMessagG[i  +  j]  1=  i++){} 

switch  (i)  { 

case  3  : 

dataSum  =  (rawMessage [ j ]  -  48)  *  100.0  + 

(rawMessage [ j+1]  -  48)  *  10.0  + 

(rawMessage [j +2]  -  48)  +  (rawMessage [j +4 ]  -  48)  *  0.1 

break; 
case  2: 

dataSum  =  (rawMessage [j ]  -  48)  *  10.0  + 

(rawMessage  [j+1]  -  48)  +  (rawMessage  [j +3  ]  -  48)  *  0.1 

break; 
case  1 : 

dataSum  =  (rawMessage  [j  ]  -  48)  +  (rawMessage  [j +2]  -  48)  *  0.  In¬ 
break  ; 

} 

return  dataSum; 

} 

//  end  of  file  compass. cpp 


O.  A2D.CFG 
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;  seqcnt :  number __of_seq_addresses_to_load 
;mode_sel: _ DIFF=1 _ SE=0 

;mode__acdc  :_Signal_coupling_select _ DC=1_AC=0 

;chcnt : _ NumbGr_of_channels_to_sequence_(hex, _1“F) 

;delta_t : _ Sample_rate_in_microsecs_3 -8192 

;samprate: _ Sample_rate_in_recurrent_mode _ 0  (fast)  -7  (slow) 

;  sampindex:_Which_channel_to_sample_in_recurrent_mode 
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0 
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0 
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A  5  2  ■  G 
B  A  2  0 
C  ■  5  2  0 
D  A  2  0 
E  5  2  0 
F  A  2  0 


P.  A2D.H 

#ifndef  _A2D_H 
#define  _A2D_H 

#include  <dos.h> 

# include  <math.h> 

#include  <conio.h> 

# include  <stdio-h> 

# include  <stdlib.h> 

# include  <stdarg.h> 

# include  <iostreain.h> 

#include  <fstream.h> 

//ESP  A2D  General  Global  Definitions 

#define  DEFEASE  0x100  //  Base  address  SEL=l->Ox300  &  SEL=0->0xl00 

#define  FIFOSIZE  1000  //  FIFO  size  (MAX=1000  decimal) 

#define  MAXCHAN  0x10  //  Max  channels 

//ESP  A2D  Status  Register  Definitions 
//BASE+02h:  OllD  DDDD 

#define  INT_STAT  0x10  //  0001  0000  INTERRUPT  STATUS  (1=IRQ  Pending) 

#define  TRG_STAT  0x08  //  0000  1000  TRIGGER  STATUS  ( l=Triggered) 

#defihe  FULL  0x01  //  0000  0001  FIFO  FULL  (001=Full) 

#define  HALF  0x05  //  0000  0101  FIFO  HALF  FULL  (101=Half  Full) 

idefine  EMPTY  0x06  //  0000  0110  FIFO  EMPTY  (110=Empty) 

//ESP  A2D  Control  Register  Definitions 
//BASE+08h:  DDDD  DDDD 
//BASE+09h:  DDDD  DDRR 

idefine  GATEIOUT  0x0008  //  0000  0000  0000  1000  GATEIOUT  (Always  Driven) 

idefine  TRG_POS  0x0010  //  0000  0000  0001  0000  TRIG  POS  (Trig  on  +I-) 

idefine  SET_TRG  0x0020  //  0000  0000  0010  0000  TRIG  SET  (Active  LOW) 

idefine  RST_TRG  0x0040  //  0000  0000  0100  0000  TRIG  CLR  (Active  LOW) 

idefine  INT_EN  0x0080  //  0000  0000  1000  0000  IRQ  ENAB  (Active  HIGH) 

idefine  DIFF  0x0400  //  0000  0100  0000  0000  DIFF/SE  (1=DIFF  0=SE) 

idefine  RMS  0x0800  //  0000  1000  0000  0000  RMS  Mode  (l=ON  0=OFF) 

idefine  CAL  0x1000  //  0001  0000  0000  0000  CAL  Mode  (l=ON  0=OFF) 

idefine  PRG_SEQ  0x1000  //  0001  0000  0000  0000  SEQ  Mode  (1=PRG  0=RUN) 
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#define  ACDC  0x2000.  //  0010  0000  0000  0000  ACDC  Mode  (1=DC  0=AC) 

#define  SAM_SEQ  0x4000  if  0100  0000  0000  0000  SAMP/SEQ  (1=SEQ  0=SAMP) 
#define  RST_FIFO  0x8000  //  1000  0000  0000  0000  FIFO  Reset (1=EN  0=REW) 

//ESP  A2D  Useful  Definitions 

#define  CLRRATE  OxFFFS  //  CLEAR  RATE  TO  HIGHEST  RATE 

//Class*  Definition  for  the  A2D  Class 
class  a2dClass  { 

public : 

a2dClass();  //  reads  a2d.cfg  file,  initializes  hardware 

-a2dClass()  {  lockTrigger ( ) ;  } 

void  readConf igFile { ) ;  //  reads  a2d.cfg  file 

void  initA2d();  //  initializes  the  a2d 

void  reinitA2d();  //  reinitializes  the  a2d  after  FIFO  overflow 

void  initSysAddr (void) ;  //  sets  address  mapping 

void  initHardware(void) ;  //  initializes  the  a2d  control  register 

//  Print  out  the  variable  ctrlw,  for  debug  purposes 
void  printCtrlw{void) ; 

//  Sets  the  A2D  Control  Register  for  Single-Ended  mode 
void  setSe (void) ; 

//  Sets  the  A2D  Control  Register  for  Differential  mode 
void  setDif f (void) ; 

//  Loads  sequencer  memory  with  channel  data 

void  setChannel (unsigned  seq, unsigned  ch, unsigned  glO, unsigned  g2) ; 

//  Sets  sequencer  to  program  mode 
void  setProgSeq (void) ; 

//  Sets  sequencer  to  run  mode 
void  setRunSeq (void) ; 

//  Loads  sequencer  address  counter  with  number  of  channels  to  scan, 
void  setCount (unsigned  nch) ; 

void  setAcDc (unsigned  acdc) ;  //  sets  AC  or  DC  coupling 

void  lockTrigger (void) ;  //  prevents  triggering 

void  unlockTrigger (void) ;  //  allows  the  trigger  to  function 
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//  Toggle  the  trigger . (software  triggering) 
void  setTrigger (void) ; 

void  resetTrigger (void) ;  //  clears  the  trigger 

//  Switches  in  the  RMS  measurement  chip 
void  setRmsOn (void) ; 

//  Switches  out  RMS  measurement  chip 
void  setRmsOf f (void) ; 

//  Sets  the  A2D  module  to  sequencer  mode 
void  setSequencer (void) ; 

//  Sets  the  A2D  module  to  sampler  mode 
void  setSamplerRate (unsigned) ; 

//  Set  GATEIOUT  bit  of  control  word  high 
void  gateloutOn (void) ; 

//  Set  GATEIOUT  bit  of  control  word  low 
void  gateloutOf f (void) ; 

//  Sets  timer  channel  1  to  square-wave  input 
void  squareWaveTimerl (unsigned) ; 

//  Initialize  the  A2D  timing  using  timer  2 
void  initTiming (unsigned  dt) ; 

void  resetFifo (void) ;  //  rewind  FIFO  to  beginning  of  memory 

void  setFifo (void) ;  //  enable  FIFO  to  acquire  data 

unsigned  getFifoStatus (void) ;  //  returns  the  state  of  the  FIFO 

//  Returns  next  data  word  stored  in  FIFO 
signed  getFifoData (void) ; 

//  Program  timer  channel  0  to  set  the  desired  interrupt  rate 
void  setIntRate (unsigned  intrate) ; 

void  intOff (void) ;  //  locks  out  the  interrupt  request  line 

void  intOn(void);  //  enables  system  interrupt  request 

//  Sets  the  trigger  level;  trigger  level  (0=-10V,  128=0V,  255=+10V) 

void  setTriggerLevel (unsigned  tl) ; 

//  Sets  falling  or  rising  edge  trigger 
void  setTriggerPosit ion (unsigned  tp) ; 
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void  zeroOff set (void) ; - 


//  calibrates  zero  offset  error 


//  Grounds  the  two  differential  inputs  for  zero  adjust 
void  grndinput (void) ; 

void  f reeinput (void) ;  II  ungrounds  the  two  differential  inputs 


void’  zeroAdjust (void) ; 


//  adjust  the  trimmer  on  the  PGA 


int  Ghent;  //  Number  of  channels  to  sequence 

unsigned  delta_t;  //  period  between  channels 


private: 


unsigned  ctrlw; 
unsigned  seqent; 
unsigned  mode_sel; 
unsigned  mode_acdc; 
unsigned  samp rate; 
unsigned  sampindex; 
unsigned  seqaddr [MAXCHAN] 
unsigned  chan [MAXCHAN] ; 
unsigned  glO [MAXCHAN] ; 
unsigned  g2 [MAXCHAN]; 


#endif 


//  Holds  A2D  Control  Register  update  values 
//  Sequence  Counter 
//  Single-ended  or  Differential 
//  AC /DC  Coupling 
//  Sample  Rate  in  Recurrent  Mode 
//  Which  Channel  to  Sample  in  Recurrent  Mode 
//  Sequencer  Address 
//  Channel 
//  xlO  Gain 
//  x2  Gain 


Q.  A2D.CPP 

# include  ‘'a2d.h" 


//ESP  A2D  Addresses 


unsigned 

BASE 

DEFBASE; 

// 

unsigned 

FIFO 

= 

0x00  ; 

// 

unsigned 

MEM 

= 

0x00  ; 

// 

unsigned 

STAT 

= 

0x02 ; 

// 

unsigned 

COUNT 

= 

0x02  ; 

// 

unsigned 

TIMERO 

= 

0x04; 

// 

unsigned 

TIMERl 

= 

0x05; 

// 

unsigned 

TIMER2 

= 

0x0  6; 

// 

unsigned 

TIMERC 

= 

0x07; 

// 

unsigned 

CNTL 

= 

0x08  ; 

// 

unsigned 

DAC 

= 

OxOC; 

// 

BASE  I/O  ADDR 

[BASE] 

0 

FIFO  READ  ADDR 

[00-01] 

(R) 

SEQUENCER  ADDR 

[00-01] 

(W) 

STATUS  REGISTER 

[02] 

(R) 

SEQUENCER  ADDR  PTR 

[02] 

(W) 

TIMER  0 

[04] 

(R/W) 

TIMER  1 

[05] 

(R/W) 

TIMER  2 

[06] 

(R/W) 

TIMER  CONTROL  WORD 

[07] 

(R/W) 

A2D  CONTROL  REGISTER 

G\ 

O 

1 

00 

O 

(W) 

DAC  DATA 

[OC] 

(W) 
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/ *********************************************************  ********* 

1 1  FUNCTION  NAME:  a2dClass ( ) 

//  AUTHOR:  Randy  Walker 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Reads  a2d.cfg  file,  initializes  address  map  and  hardware 
//  RETURNS:  void 

//  CALLS:  readConfigFileO ,  initSysAddr ( ) ,  initHardware ( ) 

//  CALLED  BY:  Object  declaration 

// 


a2dClass : :a2dClass (void) 

{ 

cerr  «  “constructing  a2dl“  «  endl; 


ctrlw=0 ; 
seqcnt=l; 
inodG_sel=0 ; 
mode_acdc=l; 
delta_t=3 ; 
chcnt=l; 
sainprate=0  ; 
sampindex=0 ; 
readConf igFile ( ) ; 
initSysAddr ( ) ; 
initHardware ( ) ; 


// 

*,*.***************.*************************************************•****** 

//  FUNCTION  NAME:  readConfigFileO 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Reads  the  a2d.cfg  file  and  sets  variables 
//  RETURNS:  void 
/ /  CALLS :  none 

//  CALLED  BY:  a2d  class  constructor 

// 

************************************************** * ********************** 


void  a2dClass :: readConf igFile ( ) 

{ 

FILE  *conf igFile; 
char  junk [ 128 ]; 

if  {(conf  igFile  =  fopen  ( ''a2d.  cfg“ ,  ”r"))  ==  NULL)  { 

fprintf (stderr ,  "Cannot  open  file  A2D.CFG. . . \n" ) ; 
exit (1) ; 

} 
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f  scanf  (conf  igFile/''  %x%s " ,  &seqcnt ,  junk)  ; 

if  (seqcnt==0  I  I  seqcnt>OxOF)  {  //  seqcnt  must  be  1-F  (15  max  in  seq  mode) 
cout  «  " \nseqcnt  out  of  range  in  A2D.CFG. . . \n" ; 
exit (1) ; 

} 

f scanf  (conf  igFile,  "%d%s" ,  Scmode^sel,  junk)  ; 
if  (mode_sel  i=0  &&  mode_sel  1=  1) { 

cout  «  "\nmode__sel  out  of  range  in  A2D .CFG,  .  .  \n“ ; 
exit ( 1) ; 

} 

f  scanf  (conf  igFile,  “%d%s'* ,  &:mode_acdc ,  junk)  ; 
if  (mode_acdc  1=0  &&  mode_acdc  !=  1){ 

cout  «  ”  \nmode_acdc  out  of  range  in  A2D.CFG.  .  .  \n'' ; 
exit (1) ; 

} 

f scanf  (conf  igFile,  •'%x%s" ,  Scchcnt ,  junk)  ; 

if  (chcnt  ==011  chcnt  >  OxOF)  {  //chcnt  must  be  1-F  (15  max  in  seq  mode) 
cout  «  "\nchcnt  out  of  range  in  A2D.CFG.  .  . \n'' ; 
exit (1) ; 

} 

f scanf (conf igFile , " %d%s " , &delta_t , junk) ; 
if  (delta_t  <311  delta_t  >  8192) { 

cout  «  ''\ndelta_t  out  of  range  in  A2D . CFG .  ,  .  \n"  ; 
exit (1) ; 

} 

if  {delta_t  <  6  ScSc  chcnt  >  1)  { 

cout  «  “\ndelta_t  must  be  >  6  for  chcnt  >  l...\n’‘; 
exit ( 1) ; 

} 

f scanf  (conf  igFile,  "%d%s" ,  Scsamprate,  junk)  ; 
if  (samprate  >  7){ 

cout  «  "Xnsamprate  out  of  range  in  A2D.CFG. . . \n“ ; 
exit (1) 7 

} 

f scanf  (conf  igFile,  " %x%s " ,  Scsampindex,  junk)  ; 
if  (sampindex  >  OxOF) { 

cout  «  •' \nsampindex  out  of  range  in  A2D.CFG.  .  ,  \n‘' ; 
exit (1) ; 

} 

for  (int  i  =  0;  i  <  seqcnt;  i++) { 

fscanf  (conf  igFile,  ‘'%x%x%x%x"  ,&seqaddr[i]  ,&chan[i]  ,£cgl0  [i]  ,Scg2[i]  )  ; 

} 

fclose (conf igFile) ; 


136 


y/****************** *************************************************** 

//  FUNCTION  NAME:  initSysAddrO 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Sets  system  address  mappings 
//  RETURNS:  void 
//  CALLS:  none 

//  CALLED  BY:  a2d  class  constructor 

/  j/ *************************************  ******************************** 

void  a2dClass : : initSysAddr (void) 

{ 

//clear  BASE 


FIFO 

&=  OxOF; 

//  FIFO  READ  ADDRESS 

[00,01] 

(R) 

MEM 

Sc=  OxOF; 

//  SEQENCER  MEM  ADDRESS 

[00,01] 

(W) 

STAT 

Sc=  OxOF; 

//  STATUS  REGISTER 

[02] 

(R) 

COUNT 

&=  OxOF; 

//  SEQENCER  ADDRESS  PTR 

[02] 

(W) 

TIMERO 

Sc-  OxOF; 

//  TIMER  0 

[04] 

(R/W) 

TIMERl 

Sc=  OxOF; 

//  TIMER  1 

[05] 

(R/W) 

TIMER2 

Sc=  OxOF; 

//  TIMER  2 

[06] 

(R/W) 

TIMERC 

Sc=  OxOF; 

//  TIMER  CONTROL  WORD 

[07] 

CR/W) 

CNTL 

Sc-  OxOF; 

//  CONTROL  REGISTER 

[08] 

(R/W) 

DAC 

Sc-  OxOF; 

//  DAC  DATA 

[OC] 

(W) 

//set  BASE 

FIFO 

1=  BASE; 

//  FIFO  READ  ADDRESS 

[00,01] 

(R) 

MEM 

1=  BASE; 

//  SEQENCER  MEM  ADDRESS 

O 

O 

O 

(W) 

STAT 

1=  BASE; 

//  STATUS  REGISTER 

[02] 

(R) 

COUNT 

1=  BASE; 

//  SEQENCER  ADDRESS  PTR 

[02] 

(W) 

TIMERO 

1=  BASE; 

//  TIMER  0 

[04] 

(R/W) 

TIMERl 

1=  BASE; 

//  TIMER  1 

[05] 

(R/W) 

TIMER2 

1=  BASE; 

//  TIMER  2 

[06] 

(R/W) 

TIMERC 

1=  BASE; 

//  TIMER  CONTROL  WORD 

[07] 

(R/W) 

CNTL 

1=  BASE; 

//  CONTROL  REGISTER 

[08] 

(R/W) 

DAC 

1=  BASE; 

//  DAC  DATA 

[OC] 

(W) 

) 


//********************************************************************* 
//  FUNCTION  NAME:  initA2d() 

//  AUTHOR:  Rick  Roberts 
//  DATE:  13  November  1996 

//  DESCRIPTION:  Performs  necessary  steps  for  initialization  of  the  a2d 
//  or  to  reinitialize  if  acceleration  parameters  are  in 

//  error  due  to  a  poor  initial  data  transfer. 

//  RETURNS:  void 

//  CALLS:  setRmsOffO,  setSequencer ( ) ,  lockTrigger ( ) ,  resetFifo(), 

//  unlockTrigger ( ) ,  and  setTrigger ( ) ,  all  in  a2d.cpp 

//  CALLED  BY:  sampler  class  constructor 

//********************************************************************* 
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void  a2dClass: :initA2d(void) 

{ 

setRmsOf f ( ) ; 
setSequencer ( ) ; 
lockTrigger ( ) ; 
resetFifo ( ) ; 
setFifo( ) ; 
unlockTrigger ( ) ; 
setT-rigger  ( )  ; 

) 


//**★*★***********★******★**★★***************************************** 
//  FUNCTION  NAME:  reinitA2d() 

//  AUTHOR:  Rick  Roberts 
//  DATE:  13  November  1996 

//  DESCRIPTION:  Perforins  necessary  steps  for  reinitialization  of  the  a2d 
//  or  to  reinitialize  if  acceleration  parameters  are  in 

//  error  due  to  a  poor  initial  data  transfer. 

//  RETURNS:  void 

//  CALLS:  readConfigFileO ,  initSysAddr { ) ,  initHardware ( ) , 2  ' 

//  setRmsOffO,  setSequencer  ()  ,  lockTrigger  ()  ,  resetFifoO, 

//  unlockTrigger 0 ,  and  setTrigger ( ) ,  all  in  a2d.cpp 

//  CALLED  BY:  sampler  class  readSamples  if  a2d  FIFO  has  overflowed 

j /*****★***********★****★**★*★****★******★***************************** 


void  a2dClass : : reinitA2d (void) 

{ 

readConf igFile ( ) ; 
initSysAddr ( ) ; 
initHardware ( ) ; 
setRmsOf  f  ( )  ; 
setSequencer ( ) ; 
lockTrigger ( ) ; 
resetFifo ( ) ; 
setFifo( ) ; 
unlockTrigger ( ) ; 
setTrigger ( ) ; 

} 


//***r^***************************************************************** 

//  FUNCTION  NAME:  initHardware ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Sets  the  A2D  Control  Register  to  0020  and  sets  the  data 
//  member,  ctrlw=0060;  initializes  the  module  setup  for 

//  software  triggering  of  the  A2D.  Programs  each  channel. 


//  RETURNS:  void 
//  CALLS:  outpwO 

//  CALLED  BY:  a2d  class  constructor 
//*★★********★★****★★*★★********************************************* 


-k-k 
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void  a2dClass : : initHardware (void) 

{ 

outpw(CNTL,SET_TRG) ; 
ctrlw  =  SET_TRGIRST_TRG; 

if  (mode_sel  ==  0) 
setSe ( ) ; 

else 

setDif f ( ) ; 

for(int  i  =  0;i  <  chcnt;i++){ 

setChannel (seqaddr [i] , chan[i] , glO I i] , g2 [i] ) ; 

} 

setAcDc (mode_acdc) ; 
initTiming(delta_t) ; 
setCount { Ghent) ; 


//********************************************************************* 
//  FUNCTION  NAME:  printCtrlwO 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Print  A2D  control  register  var,  ctrlw. 

//  The  variable  is  used  to  set  a  byte  in  the 

//  ESP  A2D  control  register  at  BASE  +  08h/09h 

//  Used  during  application  code  debug 

//  RETURNS:  void 
/ /  CALLS :  none 
//  CALLED  BY:  none 

//********************************************************************* 

void  a2dClass : :printCtrlw(void) 

{ 

printf { "ctrlw:  %04x\t“,  ctrlw); 
for  (int  i=0x00;  i  <  0x10;  i++) { 

printf  ( “%i" ,(  (ctrlw»0x0F-i)  &  1) )  ; 
if  ((i+l)%4==0) 
printf ("  "); 

} 

) 
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// 

//  FUNCTION  NAME:  setSe ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Sets  ctrlw  for  single  ended  mode  and  writes  ctrlw  to 

//  A2D  Control  Register 

//  RETURNS:  void 

//  CALLS:  outpwO 

//  CALLED  BY:  initHardware ( ) 

/ /******************************************************************* 

void  a2dClass : : setSe (void) 

{ 

ctrlw  &=  -DIFF; 
outpw{CNTL, ctrlw) ; 

} 


//******************************************************************* 
//  FUNCTION  NAME:  setDiffO 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Sets  ctrlw  for  differential  mode  and  writes  ctrlw  to 

/ /  A2D  Control  Register 

//  RETURNS:  void 

//  CALLS:  outpwO 

//  CALLED  BY:  initHardware ( ) 

//******************************************************************* 

void  a2dClass: :setDiff (void) 

{ 

ctrlw  1=  DIFF; 
outpw(CNTL, ctrlw) ; 

} 


//*******************************************»*********************** 
//  FUNCTION  NAME:  setChannel ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Loads  sequencer  memory  with  channel  data 
//  CALLS:  progSeqO,  outpw(),  runSeqO 
//  CALLED  BY:  initHardware ( ) 

//  VARIABLES:  seq  -  sequencer  number 
//  ch  -  channel  number 

//  glO  -  xlO  gain  value 

//  g2  -  x2  gain  value 

//******************************************************************* 


void  a2dClass: :setChannel (unsigned  seq, unsigned  ch, unsigned  glO, 

unsigned  g2) 


{ 

unsigned  d  =  0; 


setProgSeq{ ) ;  // 

outpw(COUNT, seq) ;  // 

7 /load  sequencer  memory 
d  1=  ch«8;  // 

d  1=  (g2«12);  // 

d  1=  (gl0«14);  // 

outpw(MEM,  d)  ;  // 

setRunSeqO;  // 

} 


set  sequencer  program  mode 
set  sequencer  address 

channel 
gain  X2 
gain  XIO 
load  sequencer 

set  sequencer  run  mode 


//*★***★★**★********★★★**★★***★*★*★★**★***★:*:*****★*★*★*★********★***★** 
//  FUNCTION  NAME:  setProgSegO 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//.DATE:  27  March  1996 

//  DESCRIPTION:  Sets  sequencer  to  program  mode 
//  RETURNS:  void 
/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  setChannel ( ) 

//***★*★★★*★**★★**★**★*****★**★★**★★★***★**★******★******************** 

void  a2dClass : : setProgSeq (void) 

{ 

ctrlw  1=  PRG_SEQ; 
outpw ( CNTL, ctrlw) ; 

} 


//★**★**★*★★★★*****★★★*******★★****★*★***★***★★*★***★★*★★*★**★★*****★** 
//  FUNCTION  NAME:  setRunSeq ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Sets  sequencer  to  run  mode 
//  RETURNS:  void 
/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  main 

//**★**************************★★★***★**★★***★★★**★★**★★***★*★*★**★★★★★ 

void  a2dClass : : setRunSeq (void) 

{ 

ctrlw  &=  -PRG_SEQ; 
outpw (CNTL, ctrlw)  ; 

} 
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j  ^  *******************•*****.***************************************  ****** 

//  FUNCTION  NAME:  setCountO 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Loads  sequencer  address  counter  with  number  of  channels 
II  to  scan . 

//  RETURNS:  vo:  ^ 

//  CALLS:  outpwO,  setProgSeqO,  setRunSeqO 
//  CALLED  BY:  initHardware ( ) 

//  VARIABLES:  nch  -  number  of  channels  to  sequence 
/ /********************************************************************* 

void  a2dClass : :setCount (unsigned  nch) 

{ 

nch=nch«4;  //  put  in  upper  nibble 

outpw(COUNT,nch) ;  //  out  to  register 

setProgSeqO ;  //  reset  sequencer 

setRunSeqO;  //  put  it  in  run  mode 

) 

//********************************************************************* 
//  FUNCTION  NAME:  setAcDc{) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Sets  AC  or  DC  Coupling 
//  RETURNS:  void 
//  CALLS:  outpwO 
//  CALLED  BY:  initHardware ( ) 

//  VARIABLES:  acdc  -  holds  coupling  value 
/ /******************************************»************************** 

void  a2dClass : :setAcDc (unsigned  acdc) 

{ 

if  (acdc) 

ctrlw  1=  ACDC;  //  acdc=l  ->  DC 

else 

ctrlw  Sc=  -ACDC;  //  acdc=0  ->  AC 

outpw(CNTL, ctrlw) ; 

} 

//********************************************************************* 
//  FUNCTION  NAME:  lockTrigger ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 
//  DESCRIPTION:  Prevents  triggering 
//  RETURNS:  void 
/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  main  » 

//********************************************************************* 
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void  a2dClass : : lockTrigger (void) 

{ 

ctrlw  &=  ~RST_TRG; 
outpw(CNTL, ctrlw)  ; 


//********************************************************************* 
//  FUNCTION  NAME:  unlockTrigger ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Allow  the  triger  to  function 
//  RETURNS:  void 
//  CALLS:  outpwO 
//  CALLED  BY:  main 

//********************************************************************* 

void  a2dClass: : unlockTrigger (void) 

{ 

ctrlw  1=  RST_TRGISET_TRG; 
outpw(CNTL, ctrlw) ; 

) 

//********************************************************************* 
//  FUNCTION  NAME:  setTriggerO 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Toggle  the  trigger  (software  triggering) 

//  RETURNS:  void 
/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  main 

//********************************************************************* 

void  a2dClass: :setTrigger(void) 

{ 

outpw (CNTL, ctrlw&~SET_TRG I RST_TRG) ; 
outpw ( CNTL , ctrlw I  SET_TRG I RST_TRG ) ; 

} 

/ ^ ********************  ************************************************* 
II  FUNCTION  NAME:  resetTrigger ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 
//  DESCRIPTION:  Clears  the  trigger 
//  RETURNS:  void 
//  CALLS:  outpw () 

//  CALLED  BY:  main 

Ij********************************************************************* 
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void  a2dClass: : resetTrigger (void) 

{ 

outpw ( CNTL , ctr Iw I SET_TRG&-RST_TRG ) ; 
outpw { CNTL , c t r Iw I SET_TRG I  RST_TRG ) ; 

) 


/ /********************************************************************* 
//  FUNCTION  NAME:  setRmsOnO 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Switches  in  the  RMS  measurement  chip 
//  RETURNS:  void 
//  CALLS:  outpw 0 
//  CALLED  BY:  main 

/ /i,  ******************************************************************** 

void  a2dClass: :setRmsOn(void) 

{ 

ctrlw  I  =  RMS  ; 
outpw(CNTL,ctrlw) ; 

} 

//********************************************************************* 
//  FUNCTION  NAME:  setRmsOf f ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Switches  out  RMS  measurement  chip 
//  RETURNS:  void 
//  CALLS:  outpw 0 
//  CALLED  BY:  main 

/ /********************************************************************* 

void  a2dClass : : setRmsOf f (void) 

{ 

ctrlw  &=  ~RMS; 
outpw (CNTL, ctrlw) ; 

) 


//********************************************************************* 
//  FUNCTION  NAME:  setSeguencer ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Sets  the  A2D  module  to  sequencer  mode 
//  RETURNS:  void 
/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  main 

//********************************************************************* 
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void  a2dClass:  :setSe<^encer(void) 

{ 

ctrlw  1=  SAM_SEQ; 
outpw(CNTL, ctrlw) ; 

) 

//****•*•**************************************************************** 
//  FUNCTION  NAME:  setSamplerRate ( ) 

//  AUTHOR;  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Sets  the  A2D  module  to  sampler  mode 

//  RETURNS:  void 

//  CALLS;  outpwO 

//  CALLED  BY:  main 

//  VARIABLES:  rate  -  sampler  rate 

//********************************************************************» 

void  a2dClass: : setSamplerRate (unsigned  rate) 

{ 

ctrlw  &=  -SAM_SEQ;  //Set  to  sampler  mode 

ctrlw  &=  CLRRATE;  //Clear  previous  rate  to  000 

ctrlw  1=  rate;  //Set  new  rate 

outpw(CNTL, ctrlw) ;  //Set  Control  Word 

) 

//********************************************************************* 
//  FUNCTION  NAME:  gateloutOnO 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Set  GATEIOUT  bit  of  control  word  high 
//  RETURNS:  void 
//  CALLS:  outpwO 
//  CALLED  BY:  main 

//********************************************************************* 

void  a2dClass : :gateloutOn (void) 

{ 

ctrlw  1=  GATEIOUT; 
outpw(CNTL, ctrlw) ; 

) 
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//********************************************************************* 
//  FUNCTION  NAME:  gateloutOf f ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Set  GATEIOUT  bit  of  control  ’.vord  low 
//  RETURNS:  void 
/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  main 

//********************************************************************* 

void  a2dClass: : gateloutOf f (void) 

{ 

ctrlw  &=  -GATEIOUT; 
outpw ( CNTL , ctrlw)  ; 

} 


/ ^ ********************************************************************* 
II  FUNCTION  NAME:  squareWaveTimerl ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Sets  timer  channel  1  to  square-wave  input 
//  RETURNS:  void 
//  CALLS:  outpO 
//  CALLED  BY:  main 

//  VARIABLES:  dt -micro  seconds  per  period  (1  to  8192) 

//  assuming  8  MHz  clock  input 

//  ch-timer  channel  1 

//  ph-local  variable 

//  pl-local  variable 

/ j ******************************  *************************************** 

void  a2dClass :: squareWaveTimerl (unsigned  dt) 

{ 

char  ph,pl; 

pi  =  (dt*8)&0xFF;  //  8  CLOCKS  PER  uS 

ph  =  (dt*8)»8; 

outp(TIMERC, 0x76) ;  //  initialize  timer 

outp(TIMERl,pl) ;  //  dt  uS  delay 

outp(TIMERl,ph) ;  //  with  8  MHz  clock 

} 
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//********************************************************************* 
//  FUNCTION  NAME:  initTiming ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Initialize  the  A2D  timing  using  timer  2 
//  RETURNS:  void 
//  CALLS:  outpO 
/ /  CALLED  BY :  initHardware ( ) 

//  VARIABLES:  dt  -  number  of  micro  seconds  (3  to  2730) 
//********************************************************************* 


void  a2dClass :: initTiming (unsigned  dt) 

{ 

char  ph,pl; 


pi  =  {dt*8)&0xFF;  //  8  CLOCKS  PER  uS 

ph  =  (dt*8)»8; 


outp (TIMERC , 0xB6 )  ; 
outp(TIMER2,pl) ; 
outp (TIMER2 , ph) ; 


//  initialize  timer2 
//  dt  uS  delay 
//  with  8  MHz  clock 


//********************************************************************* 
//  FUNCTION  NAME:  resetFifoO 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Rewind  FIFO  to  beginning  of  memory 
//  RETURNS:  void 
/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  main 

//********************************************************************* 

void  a2dClass : : resetFifo (void) 

{ 

ctrlw  &=  -RST_FIFO; 
outpw (CNTL, ctrlw) ; 

} 


//********************************************************************* 
//  FUNCTION  NAME:  setFifoO 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Enable  FIFO  to  acquire  data 
//  RETURNS:  void 
/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  main 

//********************************************************************* 
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void  a2dClass: :setFifo(void) 

{ 

ctrlw  1=  RST_FIFO; 
outpw(CNTL, ctrlw) ; 

} 

I  j -1,1,**  *********  **********************************************  ********** 

II  FUNCTION  NAME:  getFifoStatus ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 

//  DATE:  27  March  1996 

//  DESCRIPTION:  Returns  FIFO  status 

//  RETURNS:  RETURNS:  6  -  empty 

//  5  -  half  full 

//  1  -  full 

//  CALLS:  inpwO 

//  CALLED  BY:  main 

/ I********************************************************************* 

unsigned  a2dClass: :getFifoStatus (void) 

{ 

return  ( inpw ( STAT) &7 ) ; 

} 

I /********************************************************************* 

II  FUNCTION  NAME:  getFif oData ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Returns  next  data  word  stored  in  FIFO 
//  RETURNS:  16bits  of  data.  Lower  12  are  A2D  data 
//  CALLS:  inpwO 

//  CALLED  BY:  a2d  class  constructor 

//********************************************************************* 

signed  a2dClass : : getFifoData (void) 

{ 

return  ( inpw (FIFO) &0x0FFF) ;  //Get  data  and  mask  upper  nibble 

) 

II********************************************************************* 

II  FUNCTION  NAME:  setIntRateO 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Program  timer  channel  0  to  set  the  desired  interrupt  rate 
//  RETURNS:  void 
//  CALLS:  outpO 
//  CALLED  BY:  main 

//  VARIABLES:  intrate-micro  secs  per  period  (1  to  8192) 

//  assuming  8  MHz  clock  input 

ll********************** *********************************************** 
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void  a2dClass :: setlnCRate (unsigned  intrate) 

{ 

outp(TIMERC, 0x36) ;  //  Set  timer  0  to  mode  3 

outp(TIMER0, (intrate*8)&0xFF) ;  //  Load  Least  Significant  Byte 

outpCTIMERO,  (intrate*8)»8)  ;  //  Load  Most  Significant  Byte 

} 


//********************************************************************* 
//  FUNCTION  NAME:  intOffO 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Locksout  the  interupt  request  line 
//  RETURNS:  void 
//  CALLS:  outpwO 
//  CALLED  BY:  main 

/ Z********************* ********  **************************************** 

void  a2dClass : : intOf f (void) 

{ 

ctrlw  &=  -INT_EN;  //  INT_EN  is  active  high 

outpw(CNTL, ctrlw) ; 

} 


Z  /-ki,*-!,  **********************************************************  ******* 

1 1  FUNCTION  NAME:  intOn ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Enables  system  interuppt  request 
//  RETURNS:  void 
//  CALLS:  outpwO 
//  CALLED  BY:  main 

Z  z ****************************************************'** *************** 

void  a2dClass : : intOn (void) 

{ 

ctrlw  1=  INT_EN;  //  INT_EN  is  active  high 

outpw(CNTL, ctrlw) ; 

) 

zz********************************************************************* 

II  FUNCTION  NAME:  setTriggerLevel ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Sets  the  trigger  level 
//  RETURNS:  void 
//  CALLS:  outpO 
//  CALLED  BY:  main 

//  VARIABLES:  tl-trigger  level  (0=-10V,  128=0V,  255=+10V) 

ZZ********************************************************************* 
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void  a2dClass :: setTriggerLevel (unsigned  tl) 

{ 

outp(DAC,tl) ; 

) 


^^*** ******  ******  ******  ************************************************ 
1 1  FUNCTION  NAME:  setTriggerPosition { ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Sets  falling  or  rising  edge  trigger 
//  RETURNS:  void 
//  CALLS:  outpwO 
//  CALLED  BY:  main 

//  VARIABLES:  tp:  0=falling,  l=rising 

II********************************************************************* 

void  a2dClass :: setTriggerPosition (unsigned  tp) 

{ 

ctrlw  &=  ~TRG_POS;  //Clear  previous  TRG_POS 

ctrlw  1=  (tp) ?TRG_POS:0;  //Evaluate  tp  and  set  ctrlw 
outp (CNTL, ctrlw) ; 

) 


II********************************************************************* 

1 1  FUNCTION  NAME:  zeroOffsetO 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Calibrates  zero  offset  error 
//  RETURNS:  void 
//  CALLS:  outpwO 

//  CALLED  BY:  a2d  class  constructor 

II********************************************************************* 

void  a2dClass : : zeroOf f set (void) 

{ 

unsigned  d=0 , i, g2 , glO ; 
float  sum; 

float  offsetErr [4] [4] ; 

float  bits [4 ]  [4]; 

unsigned  gainsl0[4]  =  {1,10,100,100); 
unsigned  gains2[4]  =  {1,  2,  4,  8); 

clrscr ( ) ; 

printf ( ”\n\tG10\tG2\t  OFFSET\t\t  BITS"); 

for(gl0  =  0;  glO  <  4;  gl0++) 
for(g2  =  0;  g2  <  4;  g2++) 

printf (“\n\t%d\t%d\t+X.XXXXXX\t+XX.X",glO,g2) ; 
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setRmsOf f ( ) ; 
setAcDc (0) ; 
setSequencer { ) ; 
initTiiuing  (3 )  ; 
setChannel (0, 0,gl0,g2) ; 
grndinput ( ) ; 

delay(5);  //Let  new  gain  values  stabilize 

while  ( Ikbhit ( ) ) { 

for  (glO  =  0;  glO  <  4;  gl0++) { 
for  (g2  =  0;  g2  <  4;  g2++) { 
setChannel ( 0 , 0 , glO , g2 ) ; 
grndinput { ) ; 
lockTrigger ( ) ; 
resetFifo ( ) ; 
setFifo ( ) ; 
unlockTrigger ( ) ; 
setTrigger ( ) ; 
delay (1) ; 

while  (getFifoStatus ( )  1=  FULL) ; 

lockTrigger { ) ; 

for  (i  =  0,  sum  =0.0;  i  <  FIFOSIZE;  i++) { 
d=getFifoData ( ) ; 
sum+= (float )d*10/2048; 

} 

of f setErr [glO ]  [g2]  =  ( { float)  (sum/FIFOSIZE) -10  )y 

(float) (gains 10 [glO] *gains2 [g2] ) ; 

bits [glO] [g2]  = 

(float) (offsetErr [glO] [g2] *4096/20*gainsl0 [glO] *gains2 [g2] ) ; 

} 

clrscr ( ) ; 

printf (“\n\tG10\tG2\t  OFFSET\t\t  BITS"); 
for  (glO  =  0;  glO  <  4;  gl0++) { 
for  (g2  =  0;  g2  <  4;  g2++) { 

printf ( " \n\t%d\t%d\t%+l . 6f \t%+04 . If " , glO , g2 , 

of f setErr [glO] [g2] ,bits [glO] [g2] ) ; 

) 

} 

} 

f reeinput ( ) ; 
getch ( ) ; 

} 

) 
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//****★★**★★★★**★★****★*****★*★★****★★★*★★★★**★★**********★********★★*★ 
//  FUNCTION  NAME:  grndinput  ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Grounds  the  two  diff  input  for  zero  adjust 
//  RETURNS:  void 
//  CALLS:  outpwO 
//  CALLED  BY:  main 

//****★★*★*****★***★★★★★**★★*★*★★★*★★****★****★★**★*★****************** 

void  a2dClass :: grndinput (void) 

{ 

ctrlw  1=  CAL; 
outpw(CNTL,  ctrlw)  ; 

} 


j /***★★*★***********★**★***★★★******★★*★*★**★******★*★***★*★★*★★★★*★★★* 

//  FUNCTION  NAME:  freeInputO 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Ungrounds  the  two  diff  inputs 
//  RETURNS:  void 
/ /  CALLS :  outpw ( ) 

//  CALLED  BY:  main 

//***^*****************************************************:<f*********** 

void  a2dClass : : freeinput (void) 

{ 

ctrlw  &=  -CAL; 
outpw (CNTL,  ctrlw)  ; 

} 


//***★★★★**★★**★★★★★*****★★★*★*****★★*★*****★*★*****★★★******★****★★★★* 
//  FUNCTION  NAME:  zeroAdjust  ( ) 

//  AUTHOR:  Randy  Walker,  based  on  [MAXUS  95]  code 
//  DATE:  27  March  1996 

//  DESCRIPTION:  Adjust  the  trimmer  on  the  PGA 
//  RETURNS:  void 
//  CALLS:  outpw 0 
//  CALLED  BY:  main 

//★★*★★★**★**★★*****★***★*★★*★*******★**★***★************************** 

void  a2dClass :: zeroAdjust (void) 

{ 

int  i ; 

unsigned  d; 

float  sum, of f setErr ; 

clrscr ( ) ; 


152 


printf (“XnXnADJUST  THE- TRIM  POT  FOR  0.0  OFFSET\n\n" ) ; 

setRmsOf f ( ) ; 
setAcDc { 0 )  ; 
setSequencer ( ) ; 
initTiming (3) ; 

whil-e  (  Ikbhit  ( )  )  { 

setChannel ( 0 , 0 , 3 , 3 ) ; 
grndinput { ) ; 
lockTrigger ( ) ; 
resGtFifo( ) ; 
setFifo ( ) ; 
unlockTrigger ( )  ; 
setTrigger ( )  ; 

while (getFifoStatus ( )  !=  FULL) ; 

lockTrigger ( ) ; 

for  (i  =  0,  sum  =  0.0;  i  <  FIFOSIZE;  i++)  { 

d  =  getFifoData ( ) ; 
sum  +=  ( float ) d*10/2048 ; 

} 

offsetErr=( (float) (sum/FIFOSIZE)-10)/8000.0; 

printf ("\tTHE  MEASURED  DC  OFFSET  IS:  %+8 . 6f \r " , of f setErr ) ; 

} 

free Input { ) ; 
getch ( ) ; 

} 

//  end  of  file  a2d.cpp 
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APPENDIX  B:  Serial  Port  Communications  Source  Code  (C++) 


A.  GLOBALS.H 

#ifndef  _GLOBALS_H 
ttdefine  _GLOBALS_H 

# inc lude  <dos . h> 

//  types 

typedef  unsigned  charBYTE; 
typedef  unsigned  short  WORD; 
typedef  unsigned  long  DWORD; 

#define  MEM(seg,ofs)  (*((BYTE  far*)MK_FP{seg,ofs) ) ) 

#define  MEMW (seg, of s)  (*((WORD  far*)MK_FP (seg.ofs) ) ) 

enum  Boolean  {FALSE,  TRUE} ; 

//  basic  bit  twiddles 
#define  set (bit) 

#def ine  setb { data , bit ) 

#def ine  clrb (data, bit ) 

#def ine  setbit (data, bit ) 

#define  clrbit (data, bit) 

//  specific  to  ports 
#define  setportbit (reg,bit)  (outportb (reg, setb( inportb(reg) ,bit) ) ) 

#def ine  clrportbit (reg, bit)  (outportb (reg, clrb ( inportb (reg) ,bit) ) ) 

//  navigation  conversion  factors  and  useful  global  variables 
#define  MSECS_TO_DEGREES  (1.0/(1000.0  *  3600.0))  //  time  conversion 

factors 

#define  DEGREES JTOJASECS  3600000.0 
#define  MINS_TO_MSECS  60000.0 

//  Conversion  constants  for  location  of  36:35:42.2N  and  121:52: 2 8. 7W 
#define  LatToFt  0.10134  //  converts  degrees  Latitude  to  ft 

#definc  LongToFt  0.08156  //  converts  degrees  Longitude  to  ft 

#def ine  HemisphereConversion  -1  //  -1  if  west  of  of  Greenwich 

#define  RADIANMAGVAR  0.261799  //  Local  Magnetic  variation  in  radians 

#define  radToDeg  (180.0/M_PI) 

#define  degToRad  (M_PI/180.0) 


#endif 


(l«bit ) 

(data  !  set (bit)) 

(data  Sc  1  set  (bit)  ) 

(data  =  setb(data,bit) ) 
(data  =  clrb(data,bit) ) 
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B.  BUFFER.H 


#ifndef  _BUFFER_H 
#define  _BUFFER_H 

#  include  *'  toe  types .  h" 

#include  "globals .h" 

#define  ONE  (unsigned  short) 1 

/********************************************★*★************************ 


CLASS: 

AUTHOR: 

DATE: 

FUNCTION: 


buf ferClass 

Frank  Kelbe,  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 
11  July  1995 

Base  class  for  use  as  a  polymorphic  reference  in  the 
serial  port  code  which  defines  a  buffer  to  be  used  in 
serial  port  communications. 


class  bufferClass  { 


public : 


//  Constructor 

buf ferClass (WORD  sz); 

-buf ferClass ( )  {) 


//  Checks  for  the  arrival  of  new  characters  in  the  buffer 
Boolean  hasData ( )  {  return  Boolean (put Ptr  !=  getPtr) ;  } 

//  How  much  of  the  Buffer  is  used  (rounded  percentage  0  -  100) 
int  capacityUsed ( ) ; 

Boolean  Get(BYTE&);  //  read  from  the  buffer 

void  Add(BYTE);  //  write  to  the  buffer 


protected: 

//  Increment  the  pointer  to  next  position 

void  inc(WORD&  index)  {  if  (++index  ==  size)  index  =  0;  } 

WORD  before (WORD  index)  //  decrement  the  pointer 

{  return  ((index  ==  0)  ?  size  -  ONE  :  index  -  ONE);} 

WORD  get Ptr;  //  Location  of  unread  data 

WORD  putPtr;  //  Location  to  read  data  to 

WORD  size;  //  Size  of  the  buffer  in  bytes 

BYTE*  buf; 

}; 

#endif 
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C.  BUFFER.CPP 


# include  <iostream.h> 
#include  <stdio.h>- 


# include  “globals.h" 

#include  "buffer. h" 

/ /********************************************************************** 


//  FUNCTION  NAME:  bufferClass  constructor 

//  AUTHOR:  Frank  Kelbe,  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 

//  DATE:  11  July  1995 

//  DESCRIPTION:  Instantiates  a  buffer 

//  RETURNS:  void 

//  CALLS:  none 

//  CALLED  BY:  compBuffer,  GPSbuffer,  buf feredSerialPort  constructors 


// 


bufferClass: :bufferClass (WORD  sz)  :  getPtr(O),  putPtr(O),  size(sz) 

{ 

buf  =  new  BYTE [ size ] ; 

} 

//********************************************************************** 
//  FUNCTION  NAME:  capacityUsed ( ) 

//  AUTHOR:  Frank  Kelbe,  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 

//  DATE:  11  July  1995 

//  DESCRIPTION:  Returns  the  rounded  percentage  of  the  buffer  used. 

/ /  RETURNS :  void 

/ /  CALLS :  none 

//  CALLED  BY:  buf feredSerialPort : :processInterrupt 

// 


int  bufferClass : : capacityUsed ( ) 

{ 

int  cap  =  (putPtr  +  size)  %  size  -  getPtr; 
return  100  *  cap  /  size; 

} 


157 


m 


II  FUNCTION  NAME:  Get 


Frank  Kelbe,  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 
11  July  1995 

Reads  a  character  from  the  buffer 

Boolean 

hasData ( ) 

GPSbufferClass,  compBuf ferClass 


//  AUTHOR: 

//  DATE: 

//  DESCRIPTION: 

//  RETURNS: 

//  CALLS: 

//  CALLED  BY: 

// 

************************************************************ 


Boolean  buf  ferClass :  :Get  (BYTES:  data) 

{ 

if  (hasDataO)  { 

data  =  buf [getPtr] ; 
inc (getPtr)  ; 
return  TRUE; 

} 

return  FALSE; 

} 


II 


II  FUNCTION  NAME: 
//  AUTHOR: 

//  DATE: 

//  DESCRIPTION: 

// 

//  RETURNS: 

//  CALLS: 

//  CALLED  BY: 

// 


Add 

Frank  Kelbe,  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 
11  July  1995 

Writes  a  character  to  the  buffer  and  checks  for  buffer 
overflow 
void 
hasData 

GPSbufferClass,  compBuf ferClass 


void  bu ffer Class :: Add (BYTE  ch) 

{ 

buf[putPtr]  =  ch; 
inc (putPtr) ; 

if  (IhasDataO)  {  //  if  no  data  after  adding  data,  it  overflowed 
cerr  «  "\nError:  byteBuffer  overf low\n" ; 

} 

} 

//  end  of  file  buffer.cpp 
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D*  GPSBUFF.H 


#ifndef  _GPSBUFF_H 
#define  _GPSBUFF__H 

#include  "globals .h" 

# include  "toe types, h" 

# inc lude  " buf f er , h " 

#define  GPSBLOCKS  4 

#define  LINE_FEED  10 

#define  CARR_RETURN  13 

/★★★★★★★**★★*★★****★★*★*★*★*★*★***★*★★★***★★*****★*★******************** 

Class  buffers  GPS  position  messages  via  serial  port  communications. 
Uses  a  multiple  buffer  system  in  which  each  buffer  is  capable  of 
holding  a  single  position  message.  Buffers  are  filled  and  processed 
sequentially  in  a  round  robin  fashion.  Messages  are  checked  for 
validity  only  upon  attempted  reads  from  the  buffer. 

**★*★*******★*★***★**★*★★**★*★******************★**********************/ 

class  gpsBuf ferClass  :  public  bufferClass  { 

public : 

gpsBuf ferClass (BYTE  GPSblocks  =  GPSBLOCKS); 

-gpsBuf ferClass ( )  {  delete  []  block;  } 

Boolean  hasData { ) ;  //a  complete  structure  is  ready 

Boolean  Get(BYTESc)  {  return  FALSE;  } 

Boolean  Get (GPSdata) ;  //  fill  in  a  complete  structure 

void  Add(BYTE  ch)  ;  '  //  build  the  structure  byte  by  byte 

protected: 

Boolean  validHeader (GPSdata) ;  //  check  a  block  for  valid  header 

GPSdata  *block;  //  hold  the  buffered  GPS  data 

WORD  current,  last;  //  current  and  last  GPS  block  in 

use 

}; 


BYTE 


*putPlace; 


//  for  the  next  character  received 


E.  GPSBUFF.CPP 


#include  <iostreain. h> 
#include  <stdio.h> 


# include  "gpsbuff.h” 


/ 


PROGRAM: 
AUTHOR : 
DATE: 
FUNCTION: 


RETURNS : 
CALLED  BY: 
CALLS: 


gpsBuffer  (Constructor) 

Eric  Bachmann,  Dave  Gay 
11  July  1995 

Allocates  message  buffers,  indicate  that  no  data  has  been 
received  by  equalizing  current  and  last  and  set  position 
into  which  initial  character  will  be  read, 
nothing . 

navigator  class  (nav.h) 
none . 


gpsBufferClass: :gpsBufferClass(BYTE  GPSblocks)  :  current(O),  last(O), 

bufferClass (GPSblocks)  //  Call  to  base  class  constructor 

{ 

cerr  «  "constructing  gpsBuffer"  «  endl; 

block  =  new  GPSdata [GPSblocks ] ;  //  Create  an  array  of  GPSdata  elements 
putPlace  =  Sc  (block [current]  [0]  )  ;  //  Set  the  place  for  first  character 


PROGRAM: 
AUTHOR : 
DATE: 
FUNCTION: 

RETURNS : 
CALLED  BY: 
CALLS: 


Add 

Eric  Bachmann,  Dave  Gay 
11  July  1995 

Interrupt  driven  routine  which  writes  incoming  characters 

into  the  gps  buffers 

nothing. 

interupt  driven  by  buf feredSerialPort 
none . 


void  gpsBufferC lass :: Add (BYTE  data) 

{ 

static  BYTE  lastChar (data) ;  //  Holds  last  for  <cr>  <lf>  detection 

static  Boolean  IfFlag  =  FALSE;  //  True  when  message  end  is  detected 

if  (IfFlag  ScSc  (data  ==  ‘©‘))  {  //  Is  a  new  message  starting? 

last  =  current;  //  Set  last  to  buffer  with  newest  message, 

inc (current) ;  //  Set  current  to  the  next  buffer 
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//  Set  putPlace  to  the  beginning  of  the  next  buffer. 
putPlace  =  &{ block [ cur r ent ] [0] ) ; 

if Flag  =  FALSE;  //  reset  for  end  of  next  message. 


*putPlace++  =  data;  //  Write  character  into  the  buffer. 

//Has  the  end  of  a  message  been  received? 
if  (.(lastChar  ==  CARR^RETURN)  &&  (data  ==  LINE^FEED)  )  { 

If Flag  =  TRUE; 

} 

lastChar  =  data;  //Save  last  character  for  <cr>  <lf> 

detection 

} 

/*★**★*★★★*★★★***★*★**★*★★*****★*★***★********************************** 


PROGRAM: 

Get 

AUTHOR : 

Eric  Bachmann,  Dave  Gay 

DATE: 

11  July  1995 

FUNCTION: 

Checks  to  see  if  a  new  message  has  arrived,  copies  it 
the  input  argument  data  and  returns  a  flag  to  indicate 
whether  a  new  message  was  received. 

RETURNS : 

TRUE,  if  a  new  valid  position  has  been  received. 

FALSE,  otherwise 

CALLED  BY: 

navPos it  ( nav . cpp ) 
initializeNavigator  (nav. cpp) 

CALLS: 

gpsBuf ferClass : :hasData 

Boolean  gpsBuf f erClass : :Get (GPSdata  data) 

{ 

if  (hasDataO)  {  //  Has  a  new  valid  message  been 

received. 

//  Copy  the  message  out  of  the  buffer, 
memcpy  (data,  block  +  last,  GPSBLOCKSIZE) ; 

last  =  current;  //  Indicate  that  this  message  has  been  read, 

return  TRUE; 

} 

else  { 

return  FALSE; 

} 

} 
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PROGRAM:  hasData 

AUTHOR:  Eric  Bachmann,  Dave  Gay 


DATE:  11  July  1995 

FUNCTION:  Determines  whether  a  new  message  has  been  received  and 

checks  to  see  if  it  has  a  valid  header. 

RETURNS:  TRUE,  if  a  new  valid  message  has  been  received. 

CALLED  BY:  gpsBuf f erClass : : Get  (buffer. cpp) 


CALLS: 


validHeader  (buffer . cpp) 


Boolean  gpsBuf f erClass : :hasData() 

{ 

//  Has  a  new  message  with  a  valid  header  been  received 
if  (last  1=  current)  { 

if  (validHeader (block [last] ) )  { 

return  TRUE; 

} 

else  { 

return  FALSE; 

} 

} 

return  FALSE; 


PROGRAM: 

AUTHOR: 

DATE: 

FUNCTION: 

RETURNS : 
CALLED  BY: 
CALLS: 


validHeader 

Eric  Bachmann,  Dave  Gay 
11  July  1995 

Checks  to  see  if  a  message  has  the  proper  header  for  a 
Motorola  position  message.  (@@Ea) 

TRUE,  if  the  header  is  valid.  FALSE,  otherwise. 
gpsBuf f erClass; :hasData  (buffer. cpp) 
none . 


Boolean  gpsBuf f erClass :: validHeader (GPSdata  dataPtr) 

{ 

if  {(dataPtr[0]  ==  '@')  &&  (dataPtr[l]  ==  'a')  && 
(dataPtr[2]  ==  ‘E')  ScSc  (dataPtr[3]  ==  ’a‘))  { 
return  TRUE; 

} 

else  { 

return  FALSE; 

} 

} 

//  end  of  file  gpsbuff.cpp 


F.  COMPBUFF.H 


#ifndef  ^ 

_COMPBUFF_H 

#define  _ 

_COMPBUFF_H 

#include 

" toetypes .h" 

#include 

"globals.h" 

#include 

"buffer .h" 

#def ine 

COMPBLOCKS 

8 

#def ine 

LINE__FEED 

10 

#def ine 

CARR^RETURN 

13 

#def ine 

g 

103 

#def ine 

o 

111 

Class  buffers  COMPASS  messages  received  via  serial  port  communications. 
Uses  a  multiple  buffer  system  in  which  each  buffer  is  capable  of 
holding  a  single  message.  Buffers  are  filled  and  processed 
sequentially  in  a  round  robin  fashion.  Messages  are  checked  for 
validity  only  upon  attempted  reads  from  the  buffer. 


class  compBuf ferClass  :  public  bufferClass  { 
public : 

compBuf ferClass (BYTE  compBlocks  =  COMPBLOCKS) ; 

-compBuf ferClass { )  {delete  []  block;} 

Boolean  hasDataO;  //a  complete  structure  is  ready 

Boolean  Get(BYTESc)  (return  FALSE;}  //  satisfy  inheritence 
Boolean  Get (compData) ;  //  get  a  complete  structure  filled  in 

void  Add(BYTE  ch)  ;  //  build  the  structure  byte  by  byte 


protected: 


//  for  inheritance 


Boolean  validHeader (compData) ; 
compData  *block; 

WORD  current,  last; 

BYTE  *putPlace; 

}; 


//  check  a  block  for  valid  header 
//  points  to  array  of  compass  msgs 
//  current  and  last  comp  block  in  use 

//  for  the  next  character  received 


#endif 
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G.  COMPBUFF.CPP 

#include  <iostream.h> 

#include  <stdio.h> 

# inc lude  " compbuf  f . h “ 

/*★*★*★****★**★*******★**★*★**★•*•**★★************************************* 

PROGRAM:  compBuffer  (Constructor) 

AUTHOR:  Eric  Bachmann,  Randy  Walker 

DATE:  28  April  1996 

FUNCTION:  Allocates  message  buffers,  indicates  that  no  data  has  been 
received  by  equalizing  current  and  last  and  sets  the  position 
into  which  initial  character  will  be  read. 

RETURNS:  nothing. 

CALLED  BY:  compassClass  (compass. h) 


CALLS :  none . 

******************★****★*******★★★★★★★*******★★***********★***★★*******/ 


compBuf ferClass : :compBuf ferClass (BYTE  compBlocks) :  current (0) ,  last (0) , 
bufferClass (compBlocks)  //  Call  to  base  class  constructor 


{ 

cerr  «  "compBuffer  constructor  called"  «  endl; 


block  =  new  compData [compBlocks] ;  //  Create  array  of  message  buffers 
putPlace  =  &( block] currentHO]  )  ;  //  Set  position  for  first  character 

cerr  «  "compBuffer  constructed."  «  endl ; 

) 


PROGRAM:  compBuffer: :Add 

AUTHOR:  Eric  Bachmann,  Randy  Walker 


DATE:  28  April  1996 

FUNCTION:  Interrupt  driven  routine  which  writes  incoming  characters 

into  the  compass  message  buffers 
RETURNS :  nothing . 

CALLED  BY:  interrupt  driven  by  compassPort 
CALLS :  none . 

***********************»***********************************************/ 
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void  compBufferClass  r :  Add  (BYTE  data)  { 

static  Boolean  If Flag  =  FALSE;  //True,  if  message  end  detected 

static  int  messageCount ( 0 ) ;  //  Counts  characters  in  current  message 

if  (IfFlag  &Sc  (data  ==  ‘  $  ‘  )  )  {  //  Is  a  new  message  starting? 

last  =  current;  //  Set  last  to  buffer  with  newest  message, 

inc (current) ;  //  Set  current  to  the  next  buffer 

//  Set  putPlace  to  the  beginning  of  the  next  buffer, 
put Place  =  & (block [current] [0] ) ; 

IfFlag  =  FALSE;  //  reset  for  end  of  next  message. 

} 

*putPlace++  =  data;  //  Write  character  into  the  buffer. 

messageCount++ ; 


//Has  the  end  of  a  message  been  received  (<cr><lf>) ? 
if  (data  ==  LINE_FEED)  { 

IfFlag  =  TRUE; 

} 


PROGRAM: 

compBuffer: :Get 

AUTHOR: 

Eric  Bachmann,  Randy  Walker 

DATE: 

28  April  1996 

FUNCTION: 

Checks  to  see  if  a  new  message  has  arrived,  copies 

it 

into  the  input  argument  data  and  returns  a  flag  to 
whether  a  new  message  was  received. 

indicate 

RETURNS : 

TRUE,  if  a  new  valid  position  has  been  received. 
FALSE,  otherwise 

CALLED  BY; 

compass . cpp 

CALLS: 

compBuffer: :hasData 

Boolean  compBuf f erClass : :Get (compData  data) 

{ 

if  (hasDataO)  {  //  Has  a  new  valid  message  been  received. 

//  Copy  the  message  out  of  the  buffer, 
memcpy  (data,  block  +  last,  COMPSIZE) ; 

last  =  current;  //  Indicate  that  this  message  has  been  read, 

return  TRUE; 

} 

else  { 

return  FALSE; 

} 

} 
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PROGRAM:  compBuffer: :hasData 

AUTHOR:  Eric  Bachmann,  Randy  Walker 

DATE:  28  April  1996 

FUNCTION:  Determines  whether  a  new  message  has  been  received  and 

checks  to  see  if  it  has  a  valid  header. 

RETURNS:  TRUE,  if  a  new  valid  message  has  been  received. 

CALLED  BY:  compBuf f er : : Get 

CALLS:  validHeader  (compBuf f er . cpp) 


Boolean  compBuf ferClass : :hasData() 

{ 

if  {(last  1=  current)  &&  (validHeader (block [ last ])) )  { 

return  TRUE; 

} 

else  { 

return  FALSE; 

} 

} 


PROGRAM: 

AUTHOR: 

DATE: 

FUNCTION: 

RETURNS : 
CALLED  BY: 


validHeader 

Eric  Bachmann,  Dave  Gay 
11  July  1995 

Checks  to  see  if  a  message  has  the  proper  header  for  a 
compass  message.  ($C) 

TRUE,  if  the  header  is  valid.  FALSE,  otherwise. 
compBuffer: :hasData 


CALLS: 


none , 


Boolean  compBuf ferClass :: validHeader (compDat a  dataPtr) 

{ 

if  ((dataPtr[0]  ==  '$')  &&  (dataPtr[l]  ==  'C'))  { 

return  TRUE; 

} 

else  { 

return  FALSE; 

} 

} 

//end  of  file  compbuff.cpp 
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H.  SERIAL.H 

#ifndef  _SERIAL_H 
#define  _SERIAL_H 

ttinclude  <dos.h> 

#include  <stdio.h> 

#include  "globals.h" 

#define  ALMOST_FULL  80  //  %  full  to  turn  off  DTR  (user  defines) 

//  leave  the  following  alone  -  hardware  specific 

enuiti  COMport  {C0M1=1,  COM2,  COM3,  COM4}; 

enum  BaudRate  {b300,  bl200,  b2400,  b4800,  b9600}; 

enum  ParityType  {ERROR=-l,  NOPARITY,  ODD,  EVEN) ; 

enum  handshake  (NONE,  RTS_CTS,  XON_XOFF} ; 

enum  Shake  (off,  on); 

enum  interruptType  {rx_rdy,  tx_rdy,  line_stat,  modem_stat}; 

#define  BIOSMEMSEG  0x40 

#define  DLAB  0x80 

#define  IRQPORT  0x21 

tdefine  EOI  outportb ( 0x20 , 0x20 ) 

#define  COMlbase  MEMW (BIOSMEMSEG, 0) 

♦define  COM2base  MEMW (BIOSMEMSEG, 2) 

♦define  TX  (portBase) 

♦define  RX  (portBase) 

♦define  lER  (portBase+1) 

♦define  HR  (portBase+2) 

♦define  LCR  (portBase+3) 

♦define  MCR  (portBase+4) 

♦define  LSR  (portBase+5) 

♦define  MSR  (portBase+6) 

♦define  LO_LATCH  (portBase) 

♦define  HI_LATCH  (portBase+1) 

/**★★****■*:*★***★★★*★★★****★★*******★★****★****************************** 
CLASS:  serialPortClass 

AUTHOR:  Frank  Kelbe,  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 

DATE:  11  July  1995,  last  modified  January  1997 

FUNCTION:  Parent  class,  defines  a  simple  serial  port. 

class  serialPortClass  { 
public : 
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serialPortClass (COMport  port,  BaudRate  baud,  ParityType  parity, 

BYTE  wordlen,  BYTE  stopbits,  handshake  hs) ; 
-serialPortClass ( )  {} 

Boolean  Send (BYTE  data) ; 

Boolean  Get  (BYTESc  data)  ; 

inline  Boolean  dataReadyO; 

Boolean  statusChanged ( ) 

{  return  Boolean ( (ifportbit (MSR, 0)  II  ifportbit (MSR, 1 ) ) ) ;  } 


//  the  rest 

are  only  if  handshake  is  specified  as  RTS_CTS 

Boolean 

isCTSon( ) 

{  return  ifportbit (MSR, 4) ;  } 

Boolean 

isDSRon( ) 

{  return  ifportbit (MSR, 5) ;  } 

void 

setDTRon ( ) 

{  setportbit (MCR, 0 ) ;  ) 

void 

setDTRof f ( ) 

{  clrportbit (MCR, 0 ) ;  } 

void 

toggleDTR( ) ; 

void 

setRTSon ( ) 

{  setportbit (MCR, 1) ;  } 

void 

setRTSoff () 

{  clrportbit  (MCR,  1)  ) 

void 

toggleRTS { ) ; 

protected : 


WORD 

handshake 

Shake 


portBase; 

ShakeType ; 

DTRstate,  RTSstate; 


inline  Boolean  ifportbit (WORD,  BYTE) ; 

inline  void  toggle ( Shaken); 


}; 

#endif 
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I.  SERIAL.CPP 


#include  <iostream.h> 

#includG  <stdio.h> 

#include  " serial .h" 

//  Usage  Note:  Because  of  the  interrupt  handlers  used,  you  MUST  call 

//  your  compassPort  &  gpsPort  objects  port2  &  portl  so  the 

//  right  handler  gets  called  and  can  properly  service  the  interrupt. 

/*********★★********★****★*****★*★*★*★********************************** 


PROGRAM:  serialPortClass  (Constructor) 

AUTHOR:  Frank  Kelbe,  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 

DATE:  11  July  1995,  last  modified  January  1997 

FUNCTION:  Initializes  one  of  the  Serial  Ports. 

1)  Determines  the  base  I/O  port  address  for  the  given  COM  port 

2)  Sets  the  8259  IRQ  mask  value 

3)  Initializes  the  port  parameters  -  baud,  parity,  etc. 

4)  Calls  the  routine  to  initialize  interrupt  handling 

5)  Enables  DTR  and  RTS,  indicating  ready  to  go 


serialPortClass: : serialPortClass (COMport  port,  BaudRate  speed, 

ParityType  parity,  BYTE  wordlen, 

BYTE  stopbits,  handshake  hs)  : 

DTRstate(off ) ,  RTSstate (of f )  ,  ShakeType ( hs ) 

{ 

cerr  «  "serialPort  constructor  called"  «  endl; 
delay (500) ; 

switch  (port)  { 
case  COMl:  portBase 

case  COM2 :  portBase 

}  //  switch 

const  WORD  bauddiv[]  =  {0x180,  0x60,  0x30,  0x18,  OxC} ; 

/ /  Change  1 

outportb{IER, 0) ;  //  disable  UART  interrupts 

(void) inportb(LSR) ; 

(void) inportb(MSR) ; 

(void)  inportb (HR)  ; 

(void) inportb (RX) ; 

outportb(LCR,DLAB)  ;  //  set  DLAB  so  can  set  baud  rate  (read  only  port) 

outportb(LO_LATCH,  bauddiv[  speed]  &  OxFF); 
outportb(HI_LATCH,  (bauddiv[ speed]  &  OxFFOO)  »  8); 
setportbit (MCR, 3)  ;  //  turn  OUT2  on 


//  initialize  appropriate  port  base 

=  COMlbase; 

break ; 

=  COM2base; 

break; 
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BYTE  opt  =  0 ; 

if  '(parity  i=  NOPARITY)  { 

setbit (opt, 3) ;  //  enable  parity 

if  (parity  ==  EVEN)  //  set  even  parity  bit.  if  odd,  leave  bit  0 
setbit (opt, 4) ; 

} 

//  now  set  the  word  length,  len  of  5  sets  both  bits  0  and  1  to 

//  0,  6  sets  to  01,  7  to  10  and  8  to  11 

opt  1=  wordlen-S; 

opt  1=  — stopbits  «  2; 

outport b ( LCR , opt ) ; 

if  (ShakeType  ==  RTS_CTS)  { 
setDTRon ( ) ; 
setRTSon ( ) ; 

} 

cerr  «  "serialPort  constructed"  «  endl; 


PROGRAM: 
AUTHOR : 
DATE: 
FUNCTION: 


Get 

Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 
11  July  1995 

Gets  a  byte  from  the  port.  Returns  true  if  there's  one 
there,  and  fills  in  the  byte  parameter.  If  there's  no 
character,  the  parameter  is  left  alone,  and  false  is 
returned. 


Boolean  serialPortClass : :Get {BYTE&  data) 

{ 

if  (dataReadyO  )  {  //  make  sure  there '  s  a  char  there 

data  =  inportb(RX) ;  //  read  character  from  8250 

return  TRUE; 

} 

else 

return  FALSE; 
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^****************** ****** *********************************************** 
PROGRAM :  Send 

AUTHOR:  Frank  Kelbe,  Eric  Bachinann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Outputs  a  single  character  to  the  port.  Returns  Boolean 

status  indicating  whether  successful 

***********★*«*■*★*********★★*★***★**★★*★★**★*★*★★*★★★★*****★★***★**★★★★*/ 

Boolean  serialPortClass :: Send (BYTE  data) 

{ 

while  ( ! (ifportbit(LSR, 5) ) )  {};  //  wait  until  THR  ready 

switch  (ShakeType)  { 
case  NONE: 

outportb(TX, data) ; 
return  TRUE; 


case  RTS^CTS: 

if  (isCTSonO  ScSc  isDSRonO)  { 
outportb(TX,data) ; 
return  TRUE; 

} 

else  { 

return  FALSE; 

} 


//  case  XON_XOFF: 

default : 

} 

return  FALSE; 


break; 


//  add  this  later  if  needed 


/**★*★*★■*•★★★***★★****★*★*****★★★★*★*****★★★★*★★****★***★★★***★*★*★★****★ 
PROGRAM:  dataReady 

AUTHOR:  Frank  Kelbe,  Eric  Bachinann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Checks  port  to  see  if  a  character  has  arrived. 

★  *******'*-**********************************************************'^***/ 
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inline  Boolean  serialPortClass : : dataReady ( ) 

{ 

/*  Un- commenting  this  code  increases  transmission  errors,  but  this 

code  is  useful  for  troubleshooting,  so  is  retained  if  needed 
if  (ifportbit(LSR,l) )  { 

cerr  <<"\nOverrun  Error \ n " ; 

} 

if  {■ifportbit(LSR,2)  )  { 

cerr  «"\nParity  Error\n"; 

) 

if  ( ifportbit (LSR, 3 ) )  { 

cerr  «"\nFraming  Error\n“; 

} 

*/ 

return  ifportbit (LSR, 0) ; 


PROGRAM:  ifportbit 

AUTHOR:  Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 


DATE:  11  July  1995 

FUNCTION:  Checks  for  byte  on  inportb  register 


inline  Boolean  serialPortClass :: ifportbit (WORD  reg,  BYTE  bit) 

{ 

BYTE  on  =  inportb ( reg ) ; 
on  &=  set (bit); 

return  Boolean(on  ==  set (bit)); 

} 

/*********************************************************************** 


PROGRAM:  toggleDTR 

AUTHOR:  Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  toggles  Data  Transmit  Ready  if  RTS_CTS  is  off 

';e^***************************************************************/ 

void  serialPortClass : : toggleDTR ( ) 

{ 

if  (ShakeType  i=  RTS_CTS) 
return; 

if  (DTRstate  ==  off) 
setDTRon ( ) ; 
else 

setDTRoff  0 ; 
toggle (DTRstate) ; 
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/ 


PROGRAM:  toggleRTS 

AUTHOR:  Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  toggle  Ready  to  Send  (RTS)  if  RTS_CTS  is  on. 

****★★  *★★★★*•*-★**★★***********★*★********★***★**★****★*********★****•*★/ 


void  serialPortClass : : toggleRTS ( ) 

{ 

if  (ShakeType  1=  RTS_CTS) 
return; 

if  (RTSstate  ==  off) 
setRTSon { ) ; 
else 

setRTSof f ( ) ; 
toggle (RTSstate) ; 


/ 

PROGRAM:  toggle 

AUTHOR:  Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  toggles  value  of  the  input  variable 

*****************i»r***-*-**********-*‘**************************************/ 

inline  void  serialPortClass  ::  toggle  (ShakeS:  h) 

{ 

if  (h  ==  off) 
h  =  on; 

else 

h  =  off; 

} 

//  end  of  file  serial. cpp 
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J.  GPSPORT.H 


#ifndef  _GPSPORT_H 
#define  _GPSPORT_H 

#include  <dos.h> 

#include  <stdio.h> 

# include  "toetypes .h” 

#include  "globals .h" 
tinclude  "serial .h" 

# include  "gpsbuf f .h" 

//  this  is  the  type  for  a  standard  interrupt  handler 
typedef  void  interrupt  ( IntHandlerType)  (...); 

//  coin  handler  to  interface  with  process  Interrupt 
void  interrupt  COMlhandler (...); 

/★★★*★★★*★*★***★★*★*★★**★****★★*********★******************************* 


CLASS : gpsPortClass 
AUTHOR: Rick  Roberts 
DATE: 28  January  1997 

FUNCTION:  Defines  a  buffered  serial  port  which  is  interrupt  driven 

on  receive,  and  buffers  all  incoming  characters  in  the 
gps  buffer 


class  gpsPortClass  :  public  serialPortClass  { 
public : 

gpsPortClass (COMport  portnum  =  COMl,  BYTE  irq  =  4, 

BaudRate  speed  =  b9600, 

ParityType  parity  =  NO PARITY,  BYTE  wordlen  =  8, 
BYTE  stopbits  =  1, 
handshake  hs  =  XON^XOFF) ; 


-gpsPortClass ( ) ; 

Boolean  Get  (GPSdataSc  data);  //  buffered  version 

void  processinterrupt { ) ;  //  buffers  the  incoming  character 

protected: 

gpsBuf ferClass  messages; 

//  Value  to  allow  enable  PIC  interrupts  for  COM  port 
//  keep  the  original  8259  mask  register  value 


BYTE  irqbit; 
BYTE  origirq; 
BYTE  comint; 
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IntHandlerType  *origcomint;  //  keep  original  vector  for  restoring 

//  later 

//  this  allows  the  actual  handler  to  access  processinterrupt ( ) 
friend  IntHandlerType  C0M2handler; 


}; 

extern  gpsPortClass  portl; 
#endif 


K.  GPSPORT.CPP 

#include  <iostream.h> 
#include  <stdio.h> 

# include  "gpsPort.h" 


PROGRAM:  gpsPortClass  (Constructor) 

AUTHOR:  Rick  Roberts 

DATE:  28  January  1997 

FUNCTION:  Initializes  the  interrupts  for  the  gps  Serial  Port. 

1)  takes  over  the  original  COM  interrupt 

2)  sets  the  port  bits,  parity,  and  stop  bit 

3)  enables  interrupts  on  the  8250  (async  chip) 

4)  enables  the  async  interrupt  on  the  8259  PIC 


gpsPortClass :: gpsPortClass (COMport  portnum,  BYTE  irq,  BaudRate  baud, 

ParityType  parity,  BYTE  wordlen, 

BYTE  stopbits,  handshake  hs)  : 
serialPortClass (portnum,  baud,  parity,  wordlen, 

stopbits,  hs) , 

irqbit(irq) ,  comint ( irqbit+8) 


{ 

cerr  «  “gpsPort  constructor  called"  <<  endl; 


if  (ShakeType  ==  RTS_CTS)  {  //  turn  it  off  first,  it  was  enabled 

setDTRoffO;  //  in  the  base  class 

setRTSoff  0  ; 

} 


origcomint  =  getvect (comint ) ;  //  remember  the  original  vector 
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setvect (comint , COKlhandler) ; 


//  point  to  the  new  handler 


setportbit (MCR, 3) ;  //  turn  0UT2  on 

disableO;  //  disable  all  interrupts  -  critical  section 

setportbit (IER,rx_rdy) ;  if  enable  ints  on  receive  only 

origirq  =  inportb ( IRQPORT) ;  //  remember  how  it  was 

clrportbit (IRQPORT, irqbit) ;  //  enable  COM  ints 

if  (ShakeType  ==  RTS_CTS)  { 
setDTRon ( ) ; 
setRTSonO; 

) 

enable ( ) ; 

EOI; 

cerr  «  "exiting  gpsPort  constructor"  «  endl; 


^*********************************************** ************************ 
PROGRAM:  -gpsPortClass 

AUTHOR:  Rick  Roberts,  Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE:  28  January  1997 

FUNCTION:  Resets  the  interrupts. 

1)  disables  the  8250  (async  chip) 

2)  disables  the  interrupt  chip  for  async  int 

3)  resets  the  8259  PIC 

gpsPortClass : : -gpsPortClass ( ) 

{ 

setvect (comint , origcomint) ; 
outportb ( lER, 0 ) ; 
outportb (MCR, 0) ; 
outportb (IRQPORT, origirq) ; 

EOI; 


/**★***★★★***★**★*★★*****★★★★***★*************************************** 
PROGRAM:  Get 

AUTHOR:  Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Calls  Get  based  on  buffer  type 


//  set  the  interrupt  vector  back 
//  disable  further  UART  interrupts 
//  turn  everything  off 
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Boolean  gpsPortClass : :Get (GPSdata&  data) 

{ 

return  messages .Get (data) ; 

} 

/*********************************************************************** 
PROGRAM :  s howPor t  s 

AUTHOR:  Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Prints  interrupt  vector  addresses.  This  function  is  for 

trouble  shooting,  it  is  not  called  in  the  code. 


***********************************************************************/ 

/* 

int  showPorts ( ) 

{ 

BYTE*  p  =  (BYTE*)C0M2base; 
p+=5; 

fprintf  (stderr, ''%X  “,*p++); 

fprintf (stderr, "%X\n" , *p++) ; 

fprintf (stderr, "IRQPORT  =  %X“,  inportb ( IRQPORT) ) ; 
return  0 ; 

} 


PROGRAM:  COMlhandler 

AUTHOR:  Frank  Kelbe,  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 

DATE:  11  July  1995,  last  modified  January  1997 

FUNCTION:  Specific  interrupt  handler  which  maps  each  interrupt  to 

the  proper  ISR. 


void  interrupt  COMlhandler ( . . . ) 

{ 

portl .processinterrupt ( ) ; 
EOI; 


PROGRAM:  processinterrupt 

AUTHOR:  Frank  Kelbe,  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 

DATE:  11  July  1995 

FUNCTION:  Calls  the  ISR  based  upon  buffer  type 
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void  gpsPortClass : :processInterrupt () 

if  (dataReadyO )  {  //  make  sure  there’s  a  char  there 

BYTE  data  =  inportb(RX) ;  //  read  character  from  8250 

messages .Add (data) ; 

if  (ShakeType  ==  RTS_CTS  &&  messages. capacityUsedO  >  ALMOST_FULL) 
setDTRoff 0 ; 

}  • 

) 

//  end  of  file  gpsport.cpp 


L.  COMPPORT.H 

#ifndef  _COMPORT_H 
#define  _COMPORT_H 


# include  <dos.h> 
iinclude  <stdio.h> 

# include  “toetypes.h" 

#include  “globals.h" 

#include  ■ serial. h" 

# include  "compbuff.h" 

/ /  this  is  the  type  for  a  standard  interrupt  handler 
typedef  void  interrupt  (IntHandlerType) (...); 


//  com  handler  to  interface  with  processinterrupt 
void  interrupt  COM2handler (...); 


CLASS : 
AUTHOR : 
DATE: 
FUNCTION : 


compass Porte lass 
Rick  Roberts 
28  January  1997 

Defines  a  buffered  serial  port  which  is  interrupt  driven 
on  receive,  and  buffers  all  incoming  characters  in  the 
compass  buffer 


class  compassPortClass  :  public  serialPortClass  { 
friend  compassClass ; 
public: 

compassPortClass (COMport  portnum  =  COM2,  BYTE  irq  =  3, 

BaudRate  speed  =  b9600, 

ParityType  parity  =  NOPARITY,  BYTE  wordlen  =  8, 
BYTE  stopbits  =  1,  handshake  hs  =  NONE) ; 
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-compassPortClass { ) ; 


Boolean  Get  (BYTESc  data);  //  buffered  version 

void  processinterrupt { ) ;  //  buffers  the  incoming  character 


private : 

compBuf ferClass  headings; 

BYTE  irqbit;  //  Value  to  allow  enable  PIC  interrupts  for  COM  port 
BYTE  origirq;  //  keep  the  original  8259  mask  register  value 
BYTE  comint; 

IntHandlerType  *origcomint;  //  keep  original  vector  for  restoring 

//  later 

//  this  allows  the  actual  handler  to  access  processinterrupt () 
friend  IntHandlerType  C0M2handler; 


extern  compassPortClass  port2 ; 

#endif 

M.  COMPPORT.CPP 

#include  <iostream.h> 

#  inc  lude  "  compport .  h  '* 

/★★★★★★★★★★*********-*-*-*r**************************^********************** 

PROGRAM:  compassPortClass  (Constructor) 

AUTHOR:  Rick  Roberts 

DATE:  28  January  1997 

FUNCTION:  Initializes  the  interrupts  for  the  compass  Serial  Port. 

1)  takes  over  the  original  COM  interrupt 

2)  sets  the  port  bits,  parity,  and  stop  bit 

3)  enables  interrupts  on  the  8250  (async  chip) 

4)  enables  the  async  interrupt  on  the  8259  PIC 

*★★★★★★**★★***★*★★★*★***★*★*★***★★★★★** ^*******************************/ 

compassPortClass :: compassPortClass (COMport  portnum,  BYTE  irq, 

BaudRate  baud,  ParityType 
parity,  BYTE  wordlen,  BYTE 
stopbits,  handshake  hs)  : 
serialPortClass (portnum,  baud,  parity,  wordlen, 

stopbits,  hs) 


cerr  «  "compassPort  constructor  called"  «  endl; 

irqbit  =  irq; 
comint  =  irqbit  +  8; 

if  (ShakeType  ==  RTS_CTS)  { 
s'etDTRof  f  ( )  ; 
aetRTSof f { ) ; 

) 

origcomint  =  getvect (comint) ;  //  remember  the  original  vector 

setvect (comint, C0M2handler) ;  //  point  to  the  new  handler 

setportbit (MCR, 3) ;  //  turn  0UT2  on 

disableO;  //  disable  all  interrupts  -  critical  section 

setportbit (lER, rx_rdy) ;  //  enable  ints  on  receive  only 

origirq  =  inportb (IRQ PORT ) ;  //  remember  how  it  was 

clrportbit (IRQPORT, irqbit) ;  //  enable  COM  ints 

if  (ShakeType  ==  RTS_CTS)  { 
setDTRon ( ) ; 
setRTSonO  ; 

} 

enable ( ) ; 

EOI; 

cerr  «  “exiting  compassPort  constructor"  <<  endl; 


//  turn  it  off  first,  it  was  enabled 
//in  the  base  class 


PROGRAM:  -compassPort 

AUTHOR:  Rick  Roberts,  Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE:  28  January  1997 

FUNCTION:  Resets  the  interrupts. 

1)  disables  the  8250  (async  chip) 

2)  disables  the  interrupt  chip  for  async  int 

3)  resets  the  8259  PIC 


compassPortClass : : -compassPortClass ( ) 

{ 

setvect (comint, origcomint) ;  //  set  the  interrupt  vector  back 

outportbdER,  0)  ;  //  disable  further  UART  interrupts 

outportb{MCR, 0) ;  //  turn  everything  off 

outportb ( IRQPORT, origirq) ; 

EOI; 

} 
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PROGRAM:  Get 

AUTHOR:  Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Calls  Get  based  on  buffer  type 


Boolean  compassPort Class  :: Get  {BYTESc  data) 

{ 

return  headings .Get (data) ; 

} 


/**★★*★★*★******★*★★*★★*★*★★★*★**★★ ************************************* 
PROGRAM:  showPorts 

AUTHOR:  Frank  Kelbe,  Eric  Bachmann,  Dave  Gay 

DATE:  11  July  1995 

FUNCTION:  Prints  interrupt  vector  addresses.  This  function  is  for 

trouble  shooting  and  is  not  called  from  the  code. 

^*****^****************************************************************/ 

/* 

int  showPorts ( ) 

{ 

BYTE*  p  =  (BYTE*)C0M2base; 
p  +=  5; 

fprintf (stderr, "%X  " , *p++) ; 

fprintf (stderr, "%X\n" , *p++) ; 

fprintf (stderr, "IRQPORT  =  %X” ,  inportb (IRQPORT) ) ; 
return  0 ; 

} 

*/ 

PROGRAM :  C0M2handler 

AUTHOR:Frank  Kelbe,  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 
DATE: 11  July  1995,  last  modified  January  1997 

FUNCTION:  Specific  interrupt  handler  which  maps  each  interrupt  to 

the  proper  ISR. 

*****★***★★★★**★★★★★★★*★★***★*★*★*★★***★★**★******★********************/ 

void  interrupt  C0M2handler (...) 

{ 

port2 .processinterrupt ( ) ; 

EOI; 
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PROGRAM:  process Interrupt 

AUTHOR:  Frank  Kelbe,  Eric  Bachmann,  Dave  Gay,  Rick  Roberts 

DATE:  11  July  1995 

FUNCTION:  Calls  the  ISR  based  upon  buffer  type 


void  cqmpassPortClass : :processInterrupt ( ) 

{ 

if  (dataReadyO  )  {  //  make  sure  there's  a  char  there 

BYTE  data  =  inportb(RX) ;  //  read  character  from  8250 

headings . Add ( data ) ; 

if  (ShakeType  ==  RTS_CTS  &&  headings . capacityUsed( )  >  ALMOST_FULL) 
setDTRof f ( ) ; 


} 

} 

//  end  of  file  compport.cpp 


APPENDIX  C.  SANS  TILT-TABLE  TEST  TUNING  AND 
CALIBRATION  PROCEDURE 


1.  Isolate  Accelerometer  Input  From  Integrator 

“Set  Kj  to  zero. 

—Only  angular  rate  scale  factor  and  bias  effects  will  be  reflected  in  error 

2.  Choose  Initial  Bias  Weight  (biasWght) 

“Using  project  experience,  background  theory 

3.  Determine  Angular  Rate  Scale  Factor 

—Baseline  setting  is  1.0. 

“Adjust  by  determining  SANS  output  vs.  actual  angle  excursion. 
“Apply  ratio  to  current  scale  factor  to  obtain  corrected  scale  factor. 
“Commanded  tilt  table  angles  taken  as  truth 
—Scale  factor  adjusts  the  output  of  the  IMU  to  actual  tilt  results. 
“pScale  (roll),  qScale  (pitch)  rScale  (yaw) 

4.  Adjust  Gain  Value  Above  Zero 

— Re-includes  accelerometer  input  to  filter 

5.  Determine  Accelerometer  Scale  Factor 

—Same  process  as  angular  rate  scale  factor 

— xAccelScale  (pitch),  yAccelScale  (roll),  zAccelScale(yaw) 

6.  Fine  Tuning 

“Adjust  various  factors  from  1-5  above 
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